MotorStop, improve IDS, Dispensers pressure

1 files changed, 11 insertions, 4 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index a1de29b08..312f970d6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -31,6 +31,7 @@
 #include "drivers/FPGA/FPGA.h"
 
 #include "modules/thread/thread_ex.h"
+#include "modules/ids/ids_ex.h"
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -134,7 +135,7 @@ void MillisecStart(void)
     ADCAcquireStart(0,1);
 }
 
-
+uint32_t msec_millisecondCounter = 0;
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc Message;
@@ -148,7 +149,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 
     //send message to the Millisec task
     Message.messageId = OneMillisec;
-    Message.tick = UsersysTickGet();
+    Message.tick = msec_millisecondCounter++;;
     Message.msglen = sizeof(MillisecMessageStruc);
     if (MillisecMsgQ != NULL)
         Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);
@@ -226,9 +227,10 @@ uint32_t MillisecLoop(uint32_t tick)
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    bool Ten_msTick, Hundred_msTick, Onesecond_Tick;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick;
     Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
+    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     //gather Motor data from FPGA
 #ifndef EVALUATION_BOARD
@@ -342,10 +344,15 @@ uint32_t MillisecLoop(uint32_t tick)
             MillisecReadFromTempSensor(Sensor_i, NULL);
     }
 
-    if (Onesecond_Tick)
+    if (O900Millisecond_Tick)
     {
         ADC_TriggerCollection();
     }
+    if (Onesecond_Tick)
+    {
+        for (Sensor_i = 0;Sensor_i < MAX_SYSTEM_DISPENSERS;Sensor_i++)
+            CalculateDispenserPressure(Sensor_i);
+    }
 
     /*        OLD CODE
     MotorStatus_Data[MOTOR_DRYER_DRIVING] = MotorGetStatusFromFPGA(MOTOR_DRYER_DRIVING);