Extended the actuator stub

5 files changed, 98 insertions, 5 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC_DispPressSens.c b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC_DispPressSens.c
index 0e3cecf99..b1fbcbf44 100644
--- a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC_DispPressSens.c
+++ b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC_DispPressSens.c
@@ -71,9 +71,9 @@ float CalculateDispenserPressure (int DispenserId)
 
     if(data<Bits_Min)
     {
-        data = Bits_Min;//40mA 0Bar
+        data = Bits_Min;//4mA 0Bar
         //if (isDispenserInConfig(DispenserId))
-        //    Report("Dispenser - Reading wrong data from ADC",__FILE__,__LINE__,DispenserId,RpWarning,(int)data,0);
+        //  Report("Dispenser - Reading wrong data from ADC",__FILE__,__LINE__,DispenserId,RpWarning,(int)data,0);
     }
 
     temp = a[DispenserId] * data + b[DispenserId];
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 8531b1d9c..e659550a9 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
@@ -455,6 +455,46 @@ uint32_t HeadCard_Actuators_Control(ACTUATORS Act_ID, bool Direction)
     return status;
 }
 
+
+uint32_t HeadCard_Actuators_Stub(ACTUATORS Act_ID, bool Enable, bool Direction)
+{
+    uint32_t status = OK;
+
+    if(Enable == true)
+    {
+        Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x01;//Enable movement
+
+        Head_I2C_EXP4_0x46.bits.OUTPUT_ACT_DIR = Direction;
+
+        if(Act_ID == ACTIN)//0
+        {
+            Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTIN_ON  = LOW;
+        }
+        else//if(Act_ID == ACTOUT) 1
+        {
+            Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTOT_ON  = LOW;
+        }
+    }
+    else
+    {
+        Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x00;//Disable movement
+
+        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(0x42,HIGH);
+    status |= Head_Write_IO_Reg(0x46,LOW_AND_HIGH);
+
+    return status;
+}
+
 /*
 bool HeadCard_Activate_Cover_LS = false;
 
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 6474dbf97..bebbe1365 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_Control_Stub,
     Actuators_Disable,
     HeadFanControl,
     HeadMagnetAction,
@@ -150,6 +151,19 @@ void Trigger_Head_Actuators_Control(uint32_t Act_ID, bool Active_Low_Pwr, bool D
     return;
 }
 
+void Trigger_Head_Actuators_Stub(uint32_t Act_ID, bool Active_Low_Pwr, bool Direction)
+{
+    I2C_ReadingMessageStruc I2C_ReadingMessage;
+
+    I2C_ReadingMessage.messageId = Actuators_Control_Stub;
+    I2C_ReadingMessage.parameter = Act_ID;
+    I2C_ReadingMessage.parameter2 = Active_Low_Pwr;
+    I2C_ReadingMessage.parameter3 = (uint8_t *)Direction;
+    if (I2C_ReadingMsgQ != NULL)
+        Mailbox_post(I2C_ReadingMsgQ , &I2C_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
 void Trigger_Head_Actuators_Disable(void)
 {
     I2C_ReadingMessageStruc I2C_ReadingMessage;
@@ -565,6 +579,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_Control_Stub:
+            HeadCard_Actuators_Stub(I2C_ReadingMessage.parameter, (bool)I2C_ReadingMessage.parameter2,(bool)I2C_ReadingMessage.parameter3);
+            break;
         case Actuators_Disable:
             HeadCard_Actuators_Disable();
             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 23c2f13e0..8e31cf90f 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
@@ -46,6 +46,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_Stub(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);
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index 8d736f352..88c5d2d8c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -611,9 +611,44 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
 
         if((request -> delay & 0xFFF000) == 0xCAF000)//Actuator
         {
-            //HeadCard_Actuators_Control((ACTUATORS)(((request -> delay) & 0x100)>>8), /*(bool)((request -> delay & 0x10)>>4), */(bool)((request -> delay & 0x01)));//Actuators: ID, power, Direction
-            Trigger_Head_Actuators_Control((ACTUATORS)(((request -> delay) & 0x100)>>8), (bool)((request -> delay & 0x10)>>4), (bool)((request -> delay & 0x01)));//Actuators: ID, power, Direction
-            //response.progress = HeadCard_Actuators_Status((ACTUATORS)((request -> delay) & 0x100)>>8);
+            if((request -> delay & 0x00000F) >1)
+            {
+                if((request -> delay & 0xFFFFFF) == 0xCAF002)//ACTUATOR-IN down without delay and without stop the actuator
+                {
+                    Trigger_Head_Actuators_Stub(ACTIN, true, false);
+                }
+                else
+                if((request -> delay & 0xFFFFFF) == 0xCAF003)//ACTUATOR-IN up without delay and without stop the actuator
+                {
+                    Trigger_Head_Actuators_Stub(ACTIN, true, true);
+                }
+                else
+                if((request -> delay & 0xFFFFFF) == 0xCAF008)//Stop  ACTUATOR-IN
+                {
+                    Trigger_Head_Actuators_Stub(ACTIN, false, true);
+                }
+                else
+                if((request -> delay & 0xFFFFFF) == 0xCAF102)//ACTUATOR-OUT  down without delay and without stop the actuator
+                {
+                    Trigger_Head_Actuators_Stub(ACTOT, true, false);
+                }
+                else
+                if((request -> delay & 0xFFFFFF) == 0xCAF103)//ACTUATOR-OUT  up without delay and without stop the actuator
+                {
+                    Trigger_Head_Actuators_Stub(ACTOT, true, true);
+                }
+                else
+                if((request -> delay & 0xFFFFFF) == 0xCAF108)//Stop  ACTUATOR-OUT
+                {
+                    Trigger_Head_Actuators_Stub(ACTOT, false, true);
+                }
+            }
+            else
+            {
+                //HeadCard_Actuators_Control((ACTUATORS)(((request -> delay) & 0x100)>>8), /*(bool)((request -> delay & 0x10)>>4), */(bool)((request -> delay & 0x01)));//Actuators: ID, power, Direction
+                Trigger_Head_Actuators_Control((ACTUATORS)(((request -> delay) & 0x100)>>8), (bool)((request -> delay & 0x10)>>4), (bool)((request -> delay & 0x01)));//Actuators: ID, power, Direction
+                //response.progress = HeadCard_Actuators_Status((ACTUATORS)((request -> delay) & 0x100)>>8);
+            }
             response.has_progress = true;
         }
         else