update the screw enc logic

2 files changed, 16 insertions, 3 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c
index e3eaf4547..e5a2c1cec 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.c
@@ -458,6 +458,7 @@ uint32_t Read_Screw_Encoder()//the value of Screw_RotEnc.Position is legal only
     uint32_t status = OK;
 
     uint16_t Save_M = 0;
+    uint16_t Save_L = 0;
 
     SCREW_ENC_M Screw_Enc_M;
     SCREW_POS Screw_Pos;
@@ -465,13 +466,18 @@ uint32_t Read_Screw_Encoder()//the value of Screw_RotEnc.Position is legal only
     Screw_Enc_M.Word = F1_SCREW_ROTENC_M;;
     Screw_Pos.Word.M = Screw_Enc_M.Bits.M;
     Screw_Pos.Word.L = F1_SCREW_ROTENC_L;
+    //read again
     Screw_Enc_M.Word = F1_SCREW_ROTENC_M;
     Save_M = Screw_Enc_M.Bits.M;
+    Save_L = F1_SCREW_ROTENC_L;
 
     if(Save_M != Screw_Pos.Word.M)
-        status = ERROR;
-    else
-        Screw_RotEnc.Position = Screw_Pos.Position;
+    {
+        Screw_Pos.Word.M = Save_M;
+        Screw_Pos.Word.L = Save_L;
+    }
+
+    Screw_RotEnc.Position = Screw_Pos.Position;
 
     Screw_RotEnc.Index_Counter = F1_SCREW_ROTENC_I;
 
@@ -479,3 +485,9 @@ uint32_t Read_Screw_Encoder()//the value of Screw_RotEnc.Position is legal only
 }
 
 
+void Reset_Screw_Encoder()
+{
+    F1_SCREW_ROTENC_M = 0;
+}
+
+
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h
index f225227f0..e660b450a 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA.h
@@ -20,6 +20,7 @@ void Set_Speed_Sensor_TypeII_Registers(uint32_t Counter, uint32_t Prescaler);
 uint32_t Read_Speed_Sensor_TypeII();
 
 uint32_t Read_Screw_Encoder();//the value of Screw_RotEnc.Position is legal only when the status is OK
+void Reset_Screw_Encoder();
 
 typedef struct
 {