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

1 files changed, 88 insertions, 10 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
index d55a80f68..18550caa6 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -600,14 +600,25 @@ uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue)
             if(MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver)
             {
                 temp = x_SET_PARAM | x_POWERSTEP01_CONFIG;
+                temp = temp << 24;
+                if(MotorDriverResponse[_motorId].DriverMode == Voltage)
+                {
+                    temp |= 0x2E8F<<8;//voltage
+                }
+                else
+                {
+                    temp |= 0x2E8F<<8;//need to change in current mode?
+                }
+
             }
             else
             {
                 temp = x_SET_PARAM | x_CONFIG;
+                temp = temp << 24;
+                temp |= MotorsCfg[_motorId].configword<<8;
             }
 
-            temp = temp << 24;
-            temp |= MotorsCfg[_motorId].configword<<8;
+
             MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
             break;
         }
@@ -628,6 +639,13 @@ uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue)
 
         }
 
+        if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver))
+        {
+            //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01)
+            input_value*=4;
+            input_value/=10;
+        }
+
         if(input_value != 0)
         {
             temp |= input_value<<16;
@@ -654,6 +672,13 @@ uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue)
 
         }
 
+        if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver))
+        {
+            //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01)
+            input_value*=4;
+            input_value/=10;
+        }
+
         if(input_value != 0)
         {
             temp |= input_value<<16;
@@ -680,6 +705,13 @@ uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue)
 
         }
 
+        if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver))
+        {
+            //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01)
+            input_value*=4;
+            input_value/=10;
+        }
+
         if(input_value != 0)
         {
             temp |= input_value<<16;
@@ -709,6 +741,13 @@ uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue)
 
         }
 
+        if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver))
+        {
+            //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01)
+            input_value*=4;
+            input_value/=10;
+        }
+
         if(input_value != 0)
         {
             temp |= input_value<<16;
@@ -1132,6 +1171,7 @@ void FPGA_SetMotStop(TimerMotors_t _motorId)
 void FPGA_SetMotKvalHold(TimerMotors_t _motorId)
 {
     uint32_t temp;
+    uint32_t input_value = 0;
 
     temp = x_SET_PARAM | x_KVAL_HOLD;
     temp = temp << 24;
@@ -1141,14 +1181,23 @@ void FPGA_SetMotKvalHold(TimerMotors_t _motorId)
 
     if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param))
     {
-        temp |= MotorsCfg[_motorId].tvalhold<<16;
+        input_value = MotorsCfg[_motorId].tvalhold;
     }
     else
     {
-        temp |= MotorsCfg[_motorId].kvalhold<<16;
+         input_value = MotorsCfg[_motorId].kvalhold;
 
     }
 
+    if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver))
+    {
+        //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01)
+        input_value*=4;
+        input_value/=10;
+    }
+
+    temp |= input_value<<16;
+
     Fpga_Spi[_motorId].TX_MOSI = temp;
     Fpga_Spi[_motorId].AMT_OF_Words = 4;
     ReportWithPackageFilter(GeneralFilter,"FPGA SetKvalHold",__FILE__,__LINE__,_motorId,RpMessage,MotorDriverResponse[_motorId].DriverMode,0);
@@ -1160,6 +1209,8 @@ void FPGA_SetMotKvalRun(TimerMotors_t _motorId)
 {
     uint32_t temp;
 
+    uint32_t input_value = 0;
+
     temp = x_SET_PARAM | x_KVAL_RUN;
     temp = temp << 24;
 
@@ -1168,14 +1219,23 @@ void FPGA_SetMotKvalRun(TimerMotors_t _motorId)
 
     if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param))
     {
-        temp |= MotorsCfg[_motorId].tvalrun<<16;
+        input_value = MotorsCfg[_motorId].tvalrun;
     }
     else
     {
-        temp |= MotorsCfg[_motorId].kvalrun<<16;
+        input_value = MotorsCfg[_motorId].kvalrun;
+
+    }
 
+    if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver))
+    {
+        //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01)
+        input_value*=4;
+        input_value/=10;
     }
 
+    temp |= input_value<<16;
+
     Fpga_Spi[_motorId].TX_MOSI = temp;
     Fpga_Spi[_motorId].AMT_OF_Words = 4;
     ReportWithPackageFilter(GeneralFilter,"FPGA SetKvalRun",__FILE__,__LINE__,_motorId,RpMessage,temp,0);
@@ -1187,6 +1247,7 @@ void FPGA_SetMotKvalRun(TimerMotors_t _motorId)
 void FPGA_SetMotKvalAcc(TimerMotors_t _motorId)
 {
     uint32_t temp;
+    uint32_t input_value = 0;
 
     temp = x_SET_PARAM | x_KVAL_ACC;
     temp = temp << 24;
@@ -1196,14 +1257,22 @@ void FPGA_SetMotKvalAcc(TimerMotors_t _motorId)
 
     if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param))
     {
-        temp |= MotorDriverRequest[_motorId].TvalAcc<<16;
+        input_value = MotorDriverRequest[_motorId].TvalAcc;
     }
     else
     {
-        temp |= MotorDriverRequest[_motorId].KvalAcc<<16;
+        input_value = MotorDriverRequest[_motorId].KvalAcc;
+
+    }
 
+    if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver))
+    {
+        //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01)
+        input_value*=4;
+        input_value/=10;
     }
 
+    temp |= input_value<<16;
     Fpga_Spi[_motorId].TX_MOSI = temp;
     Fpga_Spi[_motorId].AMT_OF_Words = 4;
     MillisecWriteToMotor(_motorId, temp, 4, NULL);
@@ -1213,6 +1282,7 @@ void FPGA_SetMotKvalAcc(TimerMotors_t _motorId)
 void FPGA_SetMotKvalDec(TimerMotors_t _motorId)
 {
     uint32_t temp;
+    uint32_t input_value = 0;
 
     temp = x_SET_PARAM | x_KVAL_DEC;
     temp = temp << 24;
@@ -1222,14 +1292,22 @@ void FPGA_SetMotKvalDec(TimerMotors_t _motorId)
 
     if((MotorDriverResponse[_motorId].DriverMode == Current) && (Extended_Motor_Param))
     {
-        temp |= MotorDriverRequest[_motorId].TvalDec<<16;
+        input_value = MotorDriverRequest[_motorId].TvalDec;
     }
     else
     {
-        temp |= MotorDriverRequest[_motorId].KvalDec<<16;
+        input_value = MotorDriverRequest[_motorId].KvalDec;
 
     }
 
+    if((MotorDriverResponse[_motorId].DriverMode == Current) && (MotorDriverResponse[_motorId].DriverType == CombinrdMotDriver))
+    {
+        //in order use the same value as it is in L6472 (current limited to 4A instead of 10A in PowrStep01)
+        input_value*=4;
+        input_value/=10;
+    }
+
+    temp |= input_value<<16;
     Fpga_Spi[_motorId].TX_MOSI = temp;
     Fpga_Spi[_motorId].AMT_OF_Words = 4;
     FPGA_SPI_Transnit(_motorId);