Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

1 files changed, 36 insertions, 2 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index e1f8a593a..6f234e26a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -77,6 +77,7 @@ uint32_t DrawerFansStatus = 0;
 
 bool watchdogCriticalAlarm = false;
 
+MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
 MillisecMotorDataStruc ScrewMovePending = {0};
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
 MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
@@ -209,6 +210,29 @@ int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback)
 
     return OK;
 }
+
+int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback)
+{
+    //==========================
+    MSBacklog[MsecLogindex]=Data;
+    Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
+    MsecLogindex++;
+    if (MsecLogindex>=199)
+        MsecLogindex = 0;
+    //==========================
+
+    ScrewSetMaxSpeedPending.Callback = Callback;
+    ScrewSetMaxSpeedPending.Data = Data;
+    ScrewSetMaxSpeedPending.Length = Length;
+    ScrewSetMaxSpeedPending.DataRequired = false;
+    if (ScrewSetMaxSpeedPending.Active == false)
+    {
+        MotorActivity++;
+        ScrewSetMaxSpeedPending.Active = true;
+    }
+
+    return OK;
+}
 int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
     if (MotorId >= NUM_OF_MOTORS) return -1;
@@ -332,7 +356,17 @@ uint32_t MillisecLoop(uint32_t tick)
                 }
                 MotorActivity--;
             }
-            if (ScrewMovePending.Active == true)
+
+            if (ScrewSetMaxSpeedPending.Active == true)
+            {
+                MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length);
+                MotorActivity--;
+                ScrewSetMaxSpeedPending.Active = false;
+                if (ScrewSetMaxSpeedPending.Callback)
+                    ScrewSetMaxSpeedPending.Callback(Motor_i,0);
+
+            }
+            else if (ScrewMovePending.Active == true)
             {
                 MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
                 MotorActivity--;
@@ -438,7 +472,7 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
         {
-            Read_MidTank_Pressure_Sensor(i);
+            Read_MidTank_Pressure_Sensor(Disp_i);
         }
         for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {