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

1 files changed, 28 insertions, 4 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 0f9848ba1..0a092821e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -170,6 +170,7 @@ void ControlStop(void)
     ControlRestart = false;
     ADCAcquireStop();
 }
+int FPGA_ReInit_Count = 0; ///avoid too many reinitializations of motors as happens when FPGA is corrupted
 uint32_t ControlActivityLed( uint32_t Parameter1)
 {
     static bool flag = false;
@@ -189,19 +190,39 @@ uint32_t ControlActivityLed( uint32_t Parameter1)
                 if (JobIsActive())
                 {
                     JobEndReason = JOB_MOTOR_ALARM;
+                    usnprintf(AlarmReasonStr, 100, "Hardware Failure Error");
                     SendJobProgress(0.0,0,false, "Hardware Failure Error");
                     AbortJob("FPGA Watchdog Error");
                 }
-                ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
+                if (FPGA_ReInit_Count++<20)
+                {
+                    ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
+
+                    ACTIVITY_GREEN_LED_ON;
+                    MotorConfiguredTimeout = 100;
 
-                ACTIVITY_GREEN_LED_ON;
-                FPGA_SetMotorsInit();
-                Motor_ReconfigAllMotors();
+                    FPGA_SetMotorsInit();
+                    Motor_ReconfigAllMotors();
+                }
             }
         }
 		else
             ACTIVITY_GREEN_LED_OFF;
 
+        if(power.color == colorOFF) Pannel_Leds(POWER_ON_OFF,MODE_OFF);
+        if(jog.color   == colorOFF) Pannel_Leds(THREAD_JOGGING,MODE_OFF);
+        if(load.color  == colorOFF) Pannel_Leds(THREAD_JOGGING,MODE_OFF);
+        if(cart1.color == colorOFF) Pannel_Leds(CART_1,MODE_OFF);
+        if(cart2.color == colorOFF) Pannel_Leds(CART_2,MODE_OFF);
+        if(cart3.color == colorOFF) Pannel_Leds(CART_3,MODE_OFF);
+
+        if(power.color == colorON) Pannel_Leds(POWER_ON_OFF,MODE_ON);
+        if(jog.color   == colorON) Pannel_Leds(THREAD_JOGGING,MODE_ON);
+        if(load.color  == colorON) Pannel_Leds(THREAD_JOGGING,MODE_ON);
+        if(cart1.color == colorON) Pannel_Leds(CART_1,MODE_ON);
+        if(cart2.color == colorON) Pannel_Leds(CART_2,MODE_ON);
+        if(cart3.color == colorON) Pannel_Leds(CART_3,MODE_ON);
+
         if(power.color == fastBILNK)
             Pannel_Leds(POWER_ON_OFF,MODE_OFF);
         else
@@ -593,6 +614,9 @@ uint32_t ControlLowLoop(uint32_t tick)
                 continue;
             if (ControlArray[ControlLowDevice_i].ControlTiming == eOneMillisecond)
                 continue;
+            if (ControlArray[ControlLowDevice_i].StartTick == tick)
+                continue;
+
             if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)<=skipped_ticks) // run the control on exact intervals
             {
                 ControlBacklog[backlogindex]=ControlLowDevice_i;