update Actuators logic

3 files changed, 58 insertions, 19 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 05423b8d8..691f1c455 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
@@ -414,7 +414,7 @@ uint32_t HeadCard_Valve_Control(bool Direction)
     return status;
 }
 */
-uint32_t ActControlId = 0xFF;
+uint32_t ActControlId = 0xFF, ActControlId1 = 0xFF;
 
 char Global_Act_Cmd_flag = 0; //Sync - prevent reset from previous command
 
@@ -468,14 +468,47 @@ uint32_t HeadCard_Actuators_Disable(void)
     //RST
     Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x00;//Disable movement
     status |= Head_Write_IO_Reg(0x42,HIGH);
+    //Trigger_OutputsWriting();
 
     Task_sleep(5);
 
     Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x01;//Enable movement
     status |= Head_Write_IO_Reg(0x42,HIGH);
+    //Trigger_OutputsWriting();
 
     return status;
 }
+
+uint32_t HeadCard_ActControlCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) // use IfIndex for Act_ID
+{
+    uint32_t status = OK;
+
+    ACTUATORS Act_ID = (ACTUATORS)(IfIndex);
+
+    if (SafeRemoveControlCallback(ActControlId1, HeadCard_ActControlCallBackFunction )==OK)
+        ActControlId1 = 0xFF;
+    else
+        Report("Remove control callback failed",__FILE__,__LINE__,(int)ActControlId1,RpWarning,(int)HeadCard_ActControlCallBackFunction,0);
+
+    if(Act_ID == ACTIN)//0
+    {
+        Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTIN_ON  = HIGH;
+    }
+    else//if(Act_ID == ACTOUT) 1
+    {
+        Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTOT_ON  = HIGH;
+    }
+
+    Trigger_ActOnWriting();//status |= Head_Write_IO_Reg(0x46,HIGH);
+
+    Global_Act_Cmd_flag++;//Sync the callback to the command
+
+    //3sec delay - Timeout
+    ActControlId = AddControlCallback("ActuatorControl", HeadCard_ActuatorsCallBackFunction, (3* eOneSecond), TemplateDataReadCBFunction,Act_ID,0, 0 );
+
+    return status;
+}
+
 uint32_t HeadCard_Actuators_Control(ACTUATORS Act_ID, bool Direction)
 {
     uint32_t status = OK;
@@ -499,22 +532,10 @@ uint32_t HeadCard_Actuators_Control(ACTUATORS Act_ID, bool Direction)
 
     status |= Head_Write_IO_Reg(0x46,LOW_AND_HIGH);
 
-    Task_sleep(500);
+    //Task_sleep(500);
 
-    if(Act_ID == ACTIN)//0
-    {
-        Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTIN_ON  = HIGH;
-    }
-    else//if(Act_ID == ACTOUT) 1
-    {
-        Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTOT_ON  = HIGH;
-    }
-    status |= Head_Write_IO_Reg(0x46,HIGH);
-
-    Global_Act_Cmd_flag++;//Sync the callback to the command
-
-    //3sec delay - Timeout
-    ActControlId = AddControlCallback("ActuatorControl", HeadCard_ActuatorsCallBackFunction, (3* eOneSecond), TemplateDataReadCBFunction,Act_ID,0, 0 );
+    //100msec delay - (need short command - Active LOW )
+    ActControlId1 = AddControlCallback("ctuatorControl", HeadCard_ActControlCallBackFunction, (/* 5 * */ eHundredMillisecond), TemplateDataReadCBFunction,Act_ID,0, 0 );
 
     return status;
 }
@@ -534,7 +555,7 @@ uint32_t HeadCard_ActStubCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) /
     {
         Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTOT_ON  = HIGH;
     }
-    Trigger_OutputsWriting();//status |= Head_Write_IO_Reg(0x46,HIGH);
+    Trigger_ActOnWriting();//status |= Head_Write_IO_Reg(0x46,HIGH);
 
     return status;
 }
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 f126c254b..90f197587 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
     HeaterWriting,
     Head_Inputs_Reading,// I/O
     Head_Outputs_Writing,
+    Head_ACT_ON_Writing,
     Head_Init_PT100,
     Actuators_Control,
     Actuators_Control_Stub,
@@ -287,6 +288,18 @@ void Trigger_OutputsWriting(void)
 
 }
 
+void Trigger_ActOnWriting(void)
+{
+    I2C_ReadingMessageStruc I2C_ReadingMessage;
+    //uint8_t Midtank_i;
+
+    I2C_ReadingMessage.messageId = Head_ACT_ON_Writing;
+    if (I2C_ReadingMsgQ != NULL)
+        Mailbox_post(I2C_ReadingMsgQ , &I2C_ReadingMessage, BIOS_NO_WAIT);
+    return;
+
+}
+
 
 void Trigger_WHS_init_IO(void)
 {
@@ -665,8 +678,12 @@ void I2C_ReadingTask(UArg arg0, UArg arg1)
             Head_Read_IO_Reg(0x44, LOW_AND_HIGH);//BREAK__ZN1-12 + BREAK_HTIN
             Head_Read_IO_Reg(0x46, LOW_AND_HIGH);//READ HEAD CARD LS + ACT status + BREAK_HTOT
             break;
-        case Head_Outputs_Writing:
-            Head_Write_IO_Reg(0x46,HIGH);//ACT
+        case Head_Outputs_Writing: //Currently not in use
+            Head_Read_IO_Reg(0x42, LOW_AND_HIGH);//
+            Head_Read_IO_Reg(0x46, LOW_AND_HIGH);//
+            break;
+        case Head_ACT_ON_Writing:
+            Head_Write_IO_Reg(0x46,HIGH);//ACT ON
             break;
         case DispenserReadData:
             Dispenser_EEPROM_Read_Data(I2C_ReadingMessage.parameter);
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 16f86f9ed..4b12a7151 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
@@ -52,6 +52,7 @@ void Trigger_Head_Actuators_Disable(void);
 void Trigger_Head_Fan_Control(uint32_t Fan_ID, uint16_t PWM);
 void Trigger_InputsReading(void);
 void Trigger_OutputsWriting(void);
+void Trigger_ActOnWriting(void);
 void Trigger_Head_Magnet(int direction);
 void Trigger_Head_MixerValve(int direction);
 void Trigger_Head_PressureArc(bool Enable_Disable);