1 files changed, 63 insertions, 0 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 04a0026bd..b414611ff 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -4,6 +4,8 @@
 #include <DataDef.h>
 #include "include.h"
 
+#include "PMR/Hardware/UploadHardWareConfigurationRequest.pb-c.h"
+#include "PMR/Hardware/HardWareConfiguration.pb-c.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareDancer.pb-c.h"
 #include "PMR/Hardware/HardwareWinder.pb-c.h"
@@ -11,6 +13,10 @@
 #include "PMR/Printing/JobSpool.pb-c.h"
 #include "PMR/common/MessageContainer.pb-c.h"
 
+#include "Common/SWUpdate/FileSystem.h"
+#include "drivers/Flash_Memory/fatfs/ff.h"
+#include "drivers/SSI_Comm/Dancer/Dancer.h"
+
 #include "thread.h"
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
@@ -100,6 +106,63 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }
+char DancerConfigPath[50] = "0://SWInfo//DancCfg.msg";
+
+uint32_t StoreDancerConfigMessage()
+{
+    FRESULT Fresult = FR_OK;
+    HardwareConfiguration DancerConfig;
+    HardwareDancer Dancers[MAX_SYSTEM_DANCERS];
+    uint8_t* response_buffer;
+    size_t response_size = 0;
+    int Dancer_i;
+
+    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;
+        DancerConfig.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0,0);
+        DancerConfig.n_dancers++;
+    }
+    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);
+    my_free(response_buffer);
+
+    return Fresult;
+}
+uint32_t LoadDancerConfigMessage()
+{
+    void* buffer = NULL;
+    uint32_t Bytes = 0;
+    FRESULT Fresult = FR_OK;
+
+    HardwareConfiguration *DancerConfig;
+    int Dancer_i;
+
+    Fresult = FileRead(DancerConfigPath, &Bytes, &buffer);
+    if (Fresult == FR_OK)
+    {
+        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;
+        }
+        hardware_configuration__free_unpacked(DancerConfig,NULL);
+        free (buffer);
+    }
+    return Fresult;
+
+}
+
 uint32_t DancerConfigMessage(HardwareDancer * request)
 {
     uint32_t status = PASSED;