Parsing the motors status word acording to the driver type - to use for alarm handling

2 files changed, 102 insertions, 0 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index 6a51d8e10..190d564e0 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -256,6 +256,29 @@ uint32_t MotorGetADCFromFPGA(TimerMotors_t _motorId)
        #endif
 }
 
+bool MotorParseStatus(TimerMotors_t _motorId)
+{
+    if (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)
+    {
+        PStep01Status[_motorId].Reg = MotorDriverResponse[_motorId].Status;
+    }
+    else
+    if (MotorDriverResponse[_motorId].DriverType == VoltageMotDriver)
+    {
+        VolMotDriverStatus[_motorId].Reg = MotorDriverResponse[_motorId].Status;
+    }
+    else
+    if (MotorDriverResponse[_motorId].DriverType == CurrentMotDriver)
+    {
+        CurMotDriverStatus[_motorId].Reg = MotorDriverResponse[_motorId].Status;
+    }
+    else // UnKnownMotDriver
+    {
+        return ERROR;
+    }
+    return OK;
+}
+
 uint32_t MotorGetStatus(TimerMotors_t _motorId)
 {
     return MotorDriverResponse[_motorId].Status;
@@ -271,6 +294,7 @@ uint32_t MotorGetStatusFromFPGA(TimerMotors_t _motorId)
         return temp;
     #else
         FPGA_GetClrMotStat_Cmd(_motorId);
+        MotorParseStatus(_motorId);
         return OK;
     #endif
 }
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
index 97bb64667..5e39b481a 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
@@ -155,6 +155,84 @@ typedef struct
 
 }MotorDriverResponseStruct;
 
+
+// PowerStep Status register
+typedef union
+{
+    struct
+    {
+        bool HiZ                    : 1; //0
+        bool Busy                   : 1; //1
+        bool SW_F                   : 1; //2
+        bool SW_EVN                 : 1; //3
+        bool DIR                    : 1; //4
+        unsigned short MOT_STATUS   : 2; //5-6
+        bool CMD_ERROR              : 1; //7
+        bool STCK_MOD               : 1; //8
+        bool UVLO_ActiveLow         : 1; //9
+        bool UVLO_ADC_ActiveLow     : 1; //10
+        unsigned short  TH_STATUS   : 2; //11-12
+        bool OCD_ActiveLow          : 1; //13
+        bool STALL_B_ActiveLow      : 1; //14
+        bool STALL_A_ActiveLow      : 1; //15
+    }bits;
+    unsigned short Reg;
+}POWERSTEP01_STATUS;
+
+POWERSTEP01_STATUS PStep01Status[NUM_OF_MOTORS];
+
+// L6470 Status register
+typedef union
+{
+    struct
+    {
+        bool HiZ                    : 1; //0
+        bool Busy                   : 1; //1
+        bool SW_F                   : 1; //2
+        bool SW_EVN                 : 1; //3
+        bool DIR                    : 1; //4
+        unsigned short MOT_STATUS   : 2; //5-6
+        bool NOTPERF_CMD            : 1; //7
+        bool WRONG_CMD              : 1; //8
+        bool UVLO_ActiveLow         : 1; //9
+        bool TH_WRN_ActiveLow       : 1; //10
+        bool TH_SD_ActiveLow        : 1; //11
+        bool OCD_ActiveLow          : 1; //12
+        bool STEP_LOSS_A_ActiveLow  : 1; //13
+        bool STEP_LOSS_B_ActiveLow  : 1; //14
+        bool SCK_MOD                : 1; //15
+    }bits;
+    unsigned short Reg;
+}L6470_STATUS;
+
+L6470_STATUS VolMotDriverStatus[NUM_OF_MOTORS];
+
+// L6472 Status register
+typedef union
+{
+    struct
+    {
+        bool HiZ                    : 1; //0
+        bool Busy                   : 1; //1
+        bool SW_F                   : 1; //2
+        bool SW_EVN                 : 1; //3
+        bool DIR                    : 1; //4
+        unsigned short MOT_STATUS   : 2; //5-6
+        bool NOTPERF_CMD            : 1; //7
+        bool WRONG_CMD              : 1; //8
+        bool UVLO_ActiveLow         : 1; //9
+        bool TH_WRN                 : 1; //10
+        bool TH_SD                  : 1; //11
+        bool OCD_ActiveLow          : 1; //12
+        bool X_Bit_13               : 1; //13
+        bool X_Bit_14               : 1; //14
+        bool SCK_MOD                : 1; //15
+    }bits;
+    unsigned short Reg;
+}L6472_STATUS;
+
+L6472_STATUS CurMotDriverStatus[NUM_OF_MOTORS];
+
 extern MotorDriverRequestStruct MotorDriverRequest[NUM_OF_MOTORS];
 
 extern MotorDriverResponseStruct MotorDriverResponse[NUM_OF_MOTORS];