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

1 files changed, 18 insertions, 7 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 78203b236..38c0f86f1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -19,6 +19,8 @@
 #include "drivers/SSI_Comm/Dancer/Dancer.h"
 #include "drivers/Flash_ram/FlashProgram.h"
 
+#include "drivers/Flash_ram/MCU_E2Prom.h"
+
 #include "thread.h"
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
@@ -44,7 +46,6 @@ uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest)
 {
     uint32_t status = PASSED;
     TimerMotors_t Motor_i,MotorId;
-    MOTDRIVER_TYPE DriverType;
     HardwareMotor *request;
     if (HWrequest == NULL)
         return ERROR;
@@ -163,18 +164,24 @@ uint32_t StoreDancerConfigMessage()
         DancerConfig.n_dancers++;
         DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint;
     }
+    MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
+    MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint);
+    MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint);
+    Report("Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0);
+
     response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
     if (response_buffer)
     {
         response_size = hardware_configuration__pack(&DancerConfig, response_buffer);
     }
-    Fresult = FileWrite(response_buffer,response_size,DancerConfigPath);
+    Fresult = FileWrite(response_buffer,response_size,DancerConfigPath,BIOS_WAIT_FOREVER);
     EraseFlashSection(DANCERS_MAP_IN_FLASH,1024);
     for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++)
     {
         ReadAppAndProgram(DANCERS_MAP_IN_FLASH, sizeof(Dancers), Dancers);
     }
 
+
     my_free(response_buffer);
 
     return Fresult;
@@ -188,6 +195,7 @@ uint32_t LoadDancerConfigMessage()
     HardwareConfiguration *DancerConfig;
     int Dancer_i;
     HardwareDancer DancersCfg1[MAX_SYSTEM_DANCERS] = {0};
+    HardwareDancer DancersCfg2[MAX_SYSTEM_DANCERS] = {0};
 
     memcpy(DancersCfg1,(void *)DANCERS_MAP_IN_FLASH,sizeof(DancersCfg1));
 
@@ -197,16 +205,19 @@ uint32_t LoadDancerConfigMessage()
         DancerConfig = hardware_configuration__unpack(NULL, Bytes, buffer);
         for (Dancer_i = 0; Dancer_i < DancerConfig->n_dancers ; Dancer_i++)
         {
-            DancersCfg[Dancer_i].zeropoint = DancerConfig->dancers[Dancer_i]->zeropoint;
+            DancersCfg2[Dancer_i].zeropoint = DancerConfig->dancers[Dancer_i]->zeropoint;
         }
         hardware_configuration__free_unpacked(DancerConfig,NULL);
         free (buffer);
     }
+    MCU_E2PromRead(EEPROM_STORAGE_DANCER_0,&DancersCfg[0].zeropoint);
+    MCU_E2PromRead(EEPROM_STORAGE_DANCER_1,&DancersCfg[1].zeropoint);
+    MCU_E2PromRead(EEPROM_STORAGE_DANCER_2,&DancersCfg[2].zeropoint);
+
 
-    if (DancersCfg[0].zeropoint != DancersCfg1[0].zeropoint) LOG_ERROR(DancersCfg[0].zeropoint,"DancersMismatch");
-    else if (DancersCfg[1].zeropoint != DancersCfg1[1].zeropoint) LOG_ERROR(DancersCfg[1].zeropoint,"DancersMismatch");
-    else if (DancersCfg[2].zeropoint != DancersCfg1[2].zeropoint) LOG_ERROR(DancersCfg[2].zeropoint,"DancersMismatch");
-    else  LOG_ERROR(DancersCfg[0].zeropoint,"Dancers Match!!");
+    Report("Dancer 0 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg1[0].zeropoint,RpWarning,(int)DancersCfg2[0].zeropoint,0);
+    Report("Dancer 1 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[1].zeropoint,(int)DancersCfg1[1].zeropoint,RpWarning,(int)DancersCfg2[1].zeropoint,0);
+    Report("Dancer 2 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[2].zeropoint,(int)DancersCfg1[2].zeropoint,RpWarning,(int)DancersCfg2[2].zeropoint,0);
 
     return Fresult;