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

1 files changed, 27 insertions, 13 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index fb9740dec..92037d0b5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -49,11 +49,17 @@ uint32_t ThreadPreSegmentState(void *JobDetails);
 
 ////////////////////////////////////////////////////////////////////////////
 
-uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
+uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     //read value is the dancer angle
-    int i,index=MAX_THREAD_MOTORS_NUM;
-    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
+    int index=MAX_THREAD_MOTORS_NUM;
+    if (IfIndex>>8 != IfTypeThread)
+    {
+        LOG_ERROR (IfIndex, "Wrong  Interface type");
+        return 0xFFFFFFFF;
+    }
+    index = IfIndex&0xFF;
+/*    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
         if (ControlIdtoMotorId[i] == deviceID)
         {
             index = i;
@@ -64,6 +70,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
         LOG_ERROR (deviceID, "No motor  for device");
         return 0xFFFFFFFF;
     }
+    */
     if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
     {
         MotorControlConfig[index].m_mesuredParam = ReadValue;
@@ -80,10 +87,9 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
     }
-
  return OK;
 }
-uint32_t ThreadControlCBFunction(uint32_t MotorId, uint32_t ReadValue)
+uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
@@ -94,7 +100,14 @@ uint32_t ThreadControlCBFunction(uint32_t MotorId, uint32_t ReadValue)
     int Pid_Id,DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
     double NormalizedError;
-    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
+    if (IfIndex>>8 != IfTypeThread)
+    {
+        LOG_ERROR (IfIndex, "Wrong  Interface type");
+        return 0xFFFFFFFF;
+    }
+    index = IfIndex&0xFF;
+
+    /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
         if (ControlIdtoMotorId[i] == MotorId)
         {
             index = i;
@@ -104,7 +117,7 @@ uint32_t ThreadControlCBFunction(uint32_t MotorId, uint32_t ReadValue)
     {
         LOG_ERROR (MotorId, "No motor  for device");
         return 0xFFFFFFFF;
-    }
+    }*/
 
     if(MotorControlConfig[index].m_isEnabled )
     {
@@ -181,22 +194,23 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
             MotorControlConfig[Motor_i].m_preError = 0;
             MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
 
-            MotorSetDirection(HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
+            MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
     #ifdef DEBUG_TEST_FUNCTIONS
+
             if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             /*if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0);
+                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
     //            continue;
                 AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
             else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);*/
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);*/
     #else
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 continue;
-                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
             else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
     #endif
     }
     Winder_Prepare();