fix for #2741 resolved

4 files changed, 38 insertions, 13 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
index 478b8cef7..5a21a3853 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
@@ -374,10 +374,6 @@ uint32_t HeadCard_ActuatorsCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
 
     ACTUATORS Act_ID = (ACTUATORS) IfIndex;
 
-    if (SafeRemoveControlCallback(ActControlId, HeadCard_ActuatorsCallBackFunction )==OK)
-        ActControlId = 0xFF;
-    else
-        Report("Remove control callback failed",__FILE__,__LINE__,(int)ActControlId,RpWarning,(int)HeadCard_ActuatorsCallBackFunction,0);
 
    //Read movement status
     Motion_Status = HeadCard_Actuators_Status(Act_ID);
@@ -385,16 +381,17 @@ uint32_t HeadCard_ActuatorsCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
     if((Motion_Status == MOVING) && (Global_Act_Cmd_flag == Act_Cmd_flag))
     {
         //RST
-        Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x00;//Disable movement
-        status |= Head_Write_IO_Reg(0x42,HIGH);
-
-        Task_sleep(5);
-
-        Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x01;//Enable movement
-        status |= Head_Write_IO_Reg(0x42,HIGH);
-
+        Trigger_Head_Actuators_Disable();
         Counter = Counter++;
     }
+    else
+    {
+        ReportWithPackageFilter(ThreadFilter,"actuators stopped",__FILE__,__LINE__,Act_ID,RpMessage,Counter,0);
+        if (SafeRemoveControlCallback(ActControlId, HeadCard_ActuatorsCallBackFunction )==OK)
+            ActControlId = 0xFF;
+        else
+            Report("Remove control callback failed",__FILE__,__LINE__,(int)ActControlId,RpWarning,(int)HeadCard_ActuatorsCallBackFunction,0);
+    }
 
     if(Counter > 2)
     {
@@ -406,7 +403,19 @@ uint32_t HeadCard_ActuatorsCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
 
     return status;
 }
+uint32_t HeadCard_Actuators_Disable(void)
+{
+    uint32_t status = OK;
+    //RST
+    Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x00;//Disable movement
+    status |= Head_Write_IO_Reg(0x42,HIGH);
+
+    Task_sleep(5);
 
+    Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x01;//Enable movement
+    status |= Head_Write_IO_Reg(0x42,HIGH);
+
+}
 uint32_t HeadCard_Actuators_Control(ACTUATORS Act_ID, bool Direction)
 {
     uint32_t status = OK;
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h
index c431ae0df..a3a35d5d3 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h
@@ -179,7 +179,8 @@ uint32_t Head_IO_Init();
 uint32_t Head_Write_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte); //LOW/HIGH/LOW_AND_HIGH
 uint32_t Head_Read_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte); //LOW/HIGH/LOW_AND_HIGH
 uint32_t HeadCard_Toggle_PT100_SEL(uint8_t HEAD_CONFIG_PT100_PT_SEL);
-;
+
+uint32_t HeadCard_Actuators_Disable(void);
 
 uint32_t HeadCard_Actuators_Control(ACTUATORS Act_ID, bool Direction);
 uint32_t HeadCard_Actuators_Status(ACTUATORS Act_ID);
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
index 81460c3ba..64dfc0166 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
@@ -42,6 +42,7 @@ typedef enum
     Head_Outputs_Writing,
     Head_Init_PT100,
     Actuators_Control,
+    Actuators_Disable,
     HeadFanControl,
     HeadMagnetAction,
     HeadValveAction,
@@ -147,6 +148,16 @@ void Trigger_Head_Actuators_Control(uint32_t Act_ID, bool Active_Low_Pwr, bool D
     return;
 }
 
+void Trigger_Head_Actuators_Disable(void)
+{
+    I2C_ReadingMessageStruc I2C_ReadingMessage;
+    //uint8_t Midtank_i;
+
+    I2C_ReadingMessage.messageId = Actuators_Disable;
+    if (I2C_ReadingMsgQ != NULL)
+        Mailbox_post(I2C_ReadingMsgQ , &I2C_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
 void Trigger_Head_Fan_Control(uint32_t Fan_ID, uint16_t PWM)
 {
     I2C_ReadingMessageStruc I2C_ReadingMessage;
@@ -524,6 +535,9 @@ void I2C_ReadingTask(UArg arg0, UArg arg1)
         case Actuators_Control:
             HeadCard_Actuators_Control(I2C_ReadingMessage.parameter, /*(bool)I2C_ReadingMessage.parameter2,*/(bool)I2C_ReadingMessage.parameter3);
             break;
+        case Actuators_Disable:
+            HeadCard_Actuators_Disable();
+            break;
         case HeadFanControl:
             Head_Fan_PWM_Control(I2C_ReadingMessage.parameter, I2C_ReadingMessage.parameter2);
             break;
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
index d8f8e2273..244f31799 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
@@ -44,6 +44,7 @@ void Trigger_PT100_Read(void);
 void Trigger_HeaterWriting(void);
 void Trigger_Head_Init_PT100(void);
 void Trigger_Head_Actuators_Control(uint32_t Act_ID, bool Active_Low_Pwr, bool Direction);
+void Trigger_Head_Actuators_Disable(void);
 void Trigger_Head_Fan_Control(uint32_t Fan_ID, uint16_t PWM);
 void Trigger_InputsReading(void);
 void Trigger_Head_Magnet(int direction);