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

1 files changed, 27 insertions, 17 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index e4a5b362c..41daa4002 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -12,7 +12,7 @@
 #include "PMR/common/MessageContainer.pb-c.h"
 
 #include "thread.h"
-HardwareMotor  MotorsCfg[NUM_OF_MOTORS]={0};
+MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
 int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
@@ -32,22 +32,32 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
     uint32_t status = PASSED;
     TimerMotors_t Motor_i;
     Motor_i = request->hardwaremotortype;
-    /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-    {
-        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
-        {
-            Motor_i = i;
-            break;
-        }
-    }
-    if (Motor_i == 0)
-    {
-        LOG_ERROR(request->hardwarepidcontroltype,"ERROR Control Id");
-        return ERROR;
-    }*/
-    //if (Motor_i< MAX_THREAD_MOTORS_NUM)
-    //{
-        memcpy (&MotorsCfg[Motor_i],request,sizeof(HardwareMotor));
+        MotorsCfg[Motor_i].configword = request->configword;
+        MotorsCfg[Motor_i].hardwaremotortype = request->hardwaremotortype;
+        MotorsCfg[Motor_i].minfrequency = request->minfrequency;
+        MotorsCfg[Motor_i].maxfrequency = request->maxfrequency;
+        MotorsCfg[Motor_i].setmicrostep = request->setmicrostep;
+        MotorsCfg[Motor_i].microstep = request->microstep;
+        MotorsCfg[Motor_i].maxchangeslope = request->maxchangeslope;
+        MotorsCfg[Motor_i].highlengthmicrosecond = request->highlengthmicrosecond;
+        MotorsCfg[Motor_i].speedmaster = request->speedmaster;
+        MotorsCfg[Motor_i].pulseperround = request->pulseperround;
+        MotorsCfg[Motor_i].pulleyradius = request->pulleyradius;
+        MotorsCfg[Motor_i].configword = request->configword;
+        MotorsCfg[Motor_i].directionthreadwize = request->directionthreadwize;
+        MotorsCfg[Motor_i].kvalhold = request->kvalhold;
+        MotorsCfg[Motor_i].kvalrun = request->kvalrun;
+        MotorsCfg[Motor_i].kvalacc = request->kvalacc;
+        MotorsCfg[Motor_i].kvaldec = request->kvaldec;
+        MotorsCfg[Motor_i].overcurrentthreshold = request->overcurrentthreshold;
+        MotorsCfg[Motor_i].stallthreshold = request->stallthreshold;
+        MotorsCfg[Motor_i].thermalcompensationfactor = request->thermalcompensationfactor;
+        MotorsCfg[Motor_i].lowspeedoptimization = request->lowspeedoptimization;
+        MotorsCfg[Motor_i].stslp = request->stslp;
+        MotorsCfg[Motor_i].intspd = request->intspd;
+        MotorsCfg[Motor_i].fnslpacc = request->fnslpacc;
+        MotorsCfg[Motor_i].fnslpdec = request->fnslpdec;
+        MotorsCfg[Motor_i].fsspd = request->fsspd;
 
         status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
 //        if (Motor_i == MOTOR_RDRIVING)