Diff: idle_check.h

Revision:

3:7b195612e26d

Parent:

2:14b52dee1c35

--- a/idle_check.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/idle_check.h	Tue Dec 08 01:27:11 2020 +0000
@@ -1,68 +1,51 @@
 #ifndef _IDLE_CHECK_H_
 #define _IDLE_CHECK_H_
 
-#include "encoder.h"
-
-
-#define IDLE_CHECK_CNT  400 //loop(2.5ms) * IDLE_CHECK_CNT = idle check time
-
-
-uint8_t idle_check_var[100] = {0, };
-int idle_enc_data[6] = {0, };
-
-uint32_t idle_cnt = 0;
-bool idle_state = false;
+#define IDLE_CHECK_CNT  1400 //loop(2.5ms) * IDLE_CHECK_CNT = idle check time
 
-void Idle_Check_Init();
-bool Idle_Check();
-
+unsigned int ex_cmd_roll=0;
+unsigned int ex_cmd_pitch=0; 
+unsigned int ex_cmd_heave=0; 
+unsigned int ex_cmd_sway=0; 
+unsigned int ex_cmd_surge=0; 
+unsigned int ex_cmd_yaw=0;
 
-void Idle_Check_Init()
-{
-    for(int i = 0; i < 100; i++)
-    {
-        idle_check_var[i] = command[i];
-    }
-}
+bool idle_state = false;
+int idle_cnt = 0;
 
-void Idle_Enc_Check()
+void idle_check(unsigned int roll, unsigned int pitch, unsigned int heave, unsigned int sway, unsigned int surge, unsigned int yaw)
 {
-    idle_enc_data[0] = encoder_data[0];
-    idle_enc_data[1] = encoder_data[1];
-    idle_enc_data[2] = encoder_data[2];
-    idle_enc_data[3] = encoder_data[3];
-    idle_enc_data[4] = encoder_data[4];
-    idle_enc_data[5] = encoder_data[5];
-}
-
-bool Idle_Check()
-{
-    bool same_command_data = true;
     
-    for(int i = 0; i < 100; i++)
-    {
-        if(idle_check_var[i] != command[i])
+    if( roll    ==  ex_cmd_roll     &&
+        pitch   ==  ex_cmd_pitch    &&
+        heave   ==  ex_cmd_heave    &&
+        sway    ==  ex_cmd_sway     &&
+        surge   ==  ex_cmd_surge    &&
+        yaw     ==  ex_cmd_yaw )
+        {
+            idle_cnt++;
+        }else
         {
-            idle_check_var[i] = command[i];
-            same_command_data = false;
+            idle_cnt = idle_cnt - 50;
         }
-    }
-    
-    if(same_command_data == false)
-    {
-        idle_cnt = 0;
-    }
-    else
-    {
-        idle_cnt++;
-    }
-    
-    if(idle_cnt >= IDLE_CHECK_CNT)
-    {
-        return true;
-    }
-    
-    return false;
+        
+        if(idle_cnt > IDLE_CHECK_CNT)
+            idle_cnt = IDLE_CHECK_CNT;
+        if(idle_cnt < 0)
+            idle_cnt = 0;
+        
+        if(idle_cnt > 1100)
+            idle_state = true;
+        else
+            idle_state = false;
+        
+        
+        ex_cmd_roll = roll;
+        ex_cmd_pitch = pitch;
+        ex_cmd_heave = heave;
+        ex_cmd_sway = sway;
+        ex_cmd_surge = surge;
+        ex_cmd_yaw = yaw;
 }
 
 #endif
\ No newline at end of file