IDS Handling, Reports, Winder message flushing

2 files changed, 14 insertions, 0 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 67de0271f..3066153ab 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -192,6 +192,18 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
 
     return OK;
 }
+int MillisecFlushMsgQ(TimerMotors_t MotorId)
+{
+    MillisecMotorDataStruc MotorData = {0};
+    int pend = Mailbox_getNumPendingMsgs(MotorsMsgQ[MotorId]);
+    int i;
+    if (pend)
+    {
+        for (i=0;i<pend;i++)
+            Mailbox_pend(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
+    }
+   return pend;
+}
 int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
     MillisecMotorDataStruc MotorData = {0};
@@ -354,6 +366,7 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         for (Sensor_i = 0;Sensor_i < MAX_SYSTEM_DISPENSERS;Sensor_i++)
             CalculateDispenserPressure(Sensor_i);
+        FPGA_GetAllDispensersValveBusyOCD();
     }
 
     /*        OLD CODE
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index cf49e697a..807b81e98 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -19,6 +19,7 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
 int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
 
 int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, MSecFptr Callback);
+int MillisecFlushMsgQ(TimerMotors_t MotorId);
 
 uint32_t getMotorStatusData(int MotorId);
 uint32_t getMotorSpeedData(int MotorId);