test for alternative speed calculation

1 files changed, 34 insertions, 1 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c
index 64e271734..f3c351a1e 100644
--- a/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c
@@ -74,6 +74,34 @@ uint32_t Read_Speed_Sensor () // //The speed sensor, RMB14SC12BC59N , is 12bit S
     FPGA_SSI_Speed_Sensor_Receive();
     return SpeedSensorResponseS.Speed;
 }
+/********************************************************************
+*
+*    Name        : GTIME_Delta_Time_Pass
+*
+*    Parameters  : start_time.
+*
+*    Return      : time pass from start time
+*
+*    Description :
+*
+*********************************************************************/
+
+uint32_t Speed_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
+{
+    uint32_t Time_Pass;
+  #define   MAX_COUNTER 0xFFF  //12 bits
+//    #define   MAX_COUNTER 0x3FFFFF  //22 bits
+
+
+  if (Current_Read < Previous_Read)
+    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
+  else
+    Time_Pass = Current_Read - Previous_Read;
+
+  return (Time_Pass);
+}
+int Position[100] = {0};
+int PositionIndex = 0;
 
 float Calculate_Speed_Sensor_Velocity( void ) // Call the function every 10mSec (read at least once every
 {
@@ -90,11 +118,13 @@ float Calculate_Speed_Sensor_Velocity( void ) // Call the function every 10mSec
     static uint32_t Prev_Enc_Position = 0;
 
     uint32_t Enc_Position = Read_Speed_Sensor();
-    uint32_t number_of_pulses;
+    uint32_t number_of_pulses,cmp_number_of_pulses;
     float velocity;
     float temp;
 
     // for CCW Enc_Position < Prev_Enc_Position
+    cmp_number_of_pulses = Speed_Delta_Position_Pass(Prev_Enc_Position,Enc_Position);
+
     if(Prev_Enc_Position == 0)
     {
         number_of_pulses = Enc_Position;
@@ -112,6 +142,9 @@ float Calculate_Speed_Sensor_Velocity( void ) // Call the function every 10mSec
     {
         number_of_pulses = Prev_Enc_Position - Enc_Position;
     }
+    Position[PositionIndex] = cmp_number_of_pulses - number_of_pulses;
+    if (PositionIndex++ >= 99)
+        PositionIndex = 0;
 
     //uint32_t sampling_period_mSec = 10;//10mSec