Add functions to set the KVAL Hold/Run/Acc/Dec

2 files changed, 70 insertions, 0 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 ea5161b15..e245c4e15 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -931,8 +931,73 @@ void FPGA_SetMotStop(TimerMotors_t _motorId)
 //    FPGA_SPI_Transnit(_motorId);
 }
 
+void FPGA_SetMotKvalHold(TimerMotors_t _motorId)
+{
+    uint32_t temp;
+
+    temp = x_SET_PARAM | x_KVAL_HOLD;
+    temp = temp << 24;
+
+    if((MotorDriverResponse[_motorId].DriverType == CurrentMotDriver) && (MotorDriverRequest[_motorId].KvalHold > 0x7F))
+        MotorDriverRequest[_motorId].KvalHold = 0x7F;
+
+    temp |= MotorDriverRequest[_motorId].KvalHold<<16;
 
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+    Fpga_Spi[_motorId].AMT_OF_Words = 4;
+    FPGA_SPI_Transnit(_motorId);
+}
 
+void FPGA_SetMotKvalRun(TimerMotors_t _motorId)
+{
+    uint32_t temp;
+
+    temp = x_SET_PARAM | x_KVAL_RUN;
+    temp = temp << 24;
+
+    if((MotorDriverResponse[_motorId].DriverType == CurrentMotDriver) && (MotorDriverRequest[_motorId].KvalRun > 0x7F))
+        MotorDriverRequest[_motorId].KvalRun = 0x7F;
+
+    temp |= MotorDriverRequest[_motorId].KvalRun<<16;
+
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+    Fpga_Spi[_motorId].AMT_OF_Words = 4;
+    FPGA_SPI_Transnit(_motorId);
+}
+
+void FPGA_SetMotKvalAcc(TimerMotors_t _motorId)
+{
+    uint32_t temp;
+
+    temp = x_SET_PARAM | x_KVAL_ACC;
+    temp = temp << 24;
+
+    if((MotorDriverResponse[_motorId].DriverType == CurrentMotDriver) && (MotorDriverRequest[_motorId].KvalAcc > 0x7F))
+        MotorDriverRequest[_motorId].KvalAcc = 0x7F;
+
+    temp |= MotorDriverRequest[_motorId].KvalAcc<<16;
+
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+    Fpga_Spi[_motorId].AMT_OF_Words = 4;
+    FPGA_SPI_Transnit(_motorId);
+}
+
+void FPGA_SetMotKvalDec(TimerMotors_t _motorId)
+{
+    uint32_t temp;
+
+    temp = x_SET_PARAM | x_KVAL_DEC;
+    temp = temp << 24;
+
+    if((MotorDriverResponse[_motorId].DriverType == CurrentMotDriver) && (MotorDriverRequest[_motorId].KvalDec > 0x7F))
+        MotorDriverRequest[_motorId].KvalDec = 0x7F;
+
+    temp |= MotorDriverRequest[_motorId].KvalDec<<16;
+
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+    Fpga_Spi[_motorId].AMT_OF_Words = 4;
+    FPGA_SPI_Transnit(_motorId);
+}
 
 ///////////////////////////////////////////////////////////////////////////////
 /*
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
index cc9d85cca..e2937e597 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
@@ -27,6 +27,11 @@ void FPGA_SetMotPosition(TimerMotors_t _motorId);
 void FPGA_SetMotSpeed(TimerMotors_t _motorId);
 void FPGA_SetMotSpeedDirect(TimerMotors_t _motorId);
 
+void FPGA_SetMotKvalHold(TimerMotors_t _motorId);
+void FPGA_SetMotKvalRun(TimerMotors_t _motorId);
+void FPGA_SetMotKvalAcc(TimerMotors_t _motorId);
+void FPGA_SetMotKvalDec(TimerMotors_t _motorId);
+
 void FPGA_SetMotorsInit();
 uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorConfig);