Add support for goto direction.

use the function MotorGoToDir

4 files changed, 24 insertions, 1 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
index f4b22b3a0..69b6d3b3b 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
@@ -549,6 +549,10 @@ uint32_t Control_WD(bool IsEnable, unsigned char SetTimer_Steps100mSec) // Contr
 
 #ifndef EVALUATION_BOARD
 
+    #ifndef  FPGA_WATCHDOG
+        IsEnable = DISABLE;
+    #endif
+
     short WD_Enable = 0x4000 | SetTimer_Steps100mSec;
 
     if (IsEnable == DISABLE)
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 45299a57e..9f40a0ca1 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -880,6 +880,24 @@ void FPGA_SetGoToPosition(TimerMotors_t _motorId)//GoTo
 
 }
 
+void FPGA_SetGoToDirPosition(TimerMotors_t _motorId)//GoTo_DIR command
+{
+    uint32_t temp;
+
+    temp = x_GOTO_DIR | MotorDriverRequest[_motorId].Direction;
+    temp = temp << 24;
+
+    if (MotorDriverRequest[_motorId].Position > 0x3FFFFF) //TODO to check if it is needed here
+        MotorDriverRequest[_motorId].Position = 0x3FFFFF;
+
+    temp |= (MotorDriverRequest[_motorId].Position);
+    Fpga_Spi[_motorId].TX_MOSI = temp;
+
+    //MillisecWriteToMotor(_motorId, temp, 4, NULL);
+    FPGA_SPI_Transnit(_motorId);
+
+}
+
 void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov
 {
     uint32_t temp;
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 aa771d100..40dbe3948 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
@@ -45,5 +45,6 @@ void FPGA_SetGoToPosition(TimerMotors_t _motorId);//GoTo
 void Change_Motor_SPI_Direction(TimerMotors_t _motorId);
 
 void FPGA_SetMotPowerStwp01GateCfg(TimerMotors_t _motorId);
+void FPGA_SetGoToDirPosition(TimerMotors_t _motorId);//GoTo_DIR command
 
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/Motors_Driver/L6470.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/Motors_Driver/L6470.c
index f92278657..878b9e19f 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/Motors_Driver/L6470.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/Motors_Driver/L6470.c
@@ -187,7 +187,7 @@ void GoTo(unsigned long pos)
 void GoTo_DIR(byte dir, unsigned long pos)
 {
 
-    Data_To_Transfer(x_GOTO_DIR);
+    Data_To_Transfer(x_GOTO_DIR | dir);
     if (pos > 0x3FFFFF) pos = 0x3FFFFF;
     Data_To_Transfer((byte)(pos >> 16));
     Data_To_Transfer((byte)(pos >> 8));