fix alarm handling (resolves bugs #4085, #4097). dancer setup - per dancer

2 files changed, 20 insertions, 19 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 07ecf5d0b..11a92ab63 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -46,7 +46,7 @@ uint32_t ThreadEndState();
 
 bool Set_Thread_Rockers_Bypass (int value);
 
-uint32_t StoreDancerConfigMessage(void);
+uint32_t StoreDancerConfigMessage(int DancerId);
 uint32_t LoadDancerConfigMessage(void);
 
 //uint32_t MotorPidRequestMessage(HardwarePidControl* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 1920db03a..626bcde7c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -188,34 +188,35 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
 }
 char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg";
 
-uint32_t StoreDancerConfigMessage()
+uint32_t StoreDancerConfigMessage(int DancerId)
 {
     uint32_t status = OK;
   //  FRESULT Fresult = FR_OK;
     //HardwareConfiguration DancerConfig;
     HardwareDancer Dancers[MAX_SYSTEM_DANCERS];
-  //  uint8_t* response_buffer;
+#ifdef FOUR_WINDERS
+    uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2,EEPROM_STORAGE_DANCER_3,EEPROM_STORAGE_DANCER_4};
+#else
+    uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2};
+#endif
   //  size_t response_size = 0;
-    int Dancer_i;
+    int Dancer_i = DancerId;
 
     //hardware_configuration__init(&DancerConfig);
     //DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS);
-    for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++)
-    {
-        hardware_dancer__init(&Dancers[Dancer_i]);
-        //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i];
-        Dancers[Dancer_i].has_zeropoint = true;
-        Dancers[Dancer_i].hardwaredancertype = Dancer_i;
-        Dancers[Dancer_i].has_hardwaredancertype = true;
-        Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0);
-        //DancerConfig.n_dancers++;
-        DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint;
-        DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0);
-    }
+    hardware_dancer__init(&Dancers[Dancer_i]);
+    //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i];
+    Dancers[Dancer_i].has_zeropoint = true;
+    Dancers[Dancer_i].hardwaredancertype = Dancer_i;
+    Dancers[Dancer_i].has_hardwaredancertype = true;
+    Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0);
+    //DancerConfig.n_dancers++;
+    DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint;
+    DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0);
+
+
 
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint);
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint);
+    status |= MCU_E2PromProgram(DancerAddress[Dancer_i],DancersCfg[Dancer_i].zeropoint);
     Report("~~~~~~Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0);
 #ifdef FOUR_WINDERS
     status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_3,DancersCfg[HARDWARE_DANCER_3].zeropoint);