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

1 files changed, 13 insertions, 5 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index 99cb83885..d5777a88a 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -301,7 +301,7 @@ uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t
     //start control:
 //uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t Parameter1, uint32_t Parameter2 )
 
-    uint32_t ControlId = AddControlCallback( MotorMoveCallBackFunction,  eTenMilliSecond, MotorControlGetnBusyState, _motorId, 0 );
+    uint32_t ControlId = AddControlCallback( MotorMoveCallBackFunction,  eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 );
     return ControlId;
 }
 
@@ -313,17 +313,25 @@ uint32_t MotorSetSpeedWithCallback (TimerMotors_t _motorId, uint32_t _freq, int
     return OK;
 }
 
-uint32_t MotorMoveCallBackFunction(uint32_t deviceId, uint32_t BusyFlag) //TODO
+uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO
 {
+    uint32_t MotorId;
+    if (IfIndex>>8 != IfTypeMotors)
+    {
+        LOG_ERROR (IfIndex, "Wrong  Interface type");
+        return 0xFFFFFFFF;
+    }
+    MotorId = IfIndex&0xFF;
+
        if (BusyFlag == 1)
        {
            //stop this control loop
-           RemoveControlCallback(deviceId, MotorMoveCallBackFunction );
+           RemoveControlCallback(MotorId, MotorMoveCallBackFunction );
            //possibly: start regular control (speed etc)
-           //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback,  eOneMillisecond, NULL, deviceId, Parameter );
+           //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback,  eOneMillisecond, (IfTypeMotors*0x100+_motorId), deviceId, Parameter );
 
            //call the module callback
-           MotorMoveModuleCallback(deviceId,BusyFlag);
+           MotorMoveModuleCallback(MotorId,BusyFlag);
 
        }
    return OK;