ArduSub: improve turn counting

Author: ES-Alexander

ArduSub/GCS_MAVLink_Sub.cpp

@@ -133,7 +133,7 @@ bool GCS_MAVLINK_Sub::send_info()
 
     CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
     send_named_float("TetherTrn",
-                     sub.quarter_turn_count/4);
+                     (float)sub.quarter_turn_count / 4);
 
     CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
     send_named_float("Lights1",

ArduSub/turn_counter.cpp

@@ -5,12 +5,12 @@
 // Count total vehicle turns to avoid tangling tether
 void Sub::update_turn_counter()
 {
-    // Determine state
+        // Determine state
     // 0: 0-90 deg, 1: 90-180 deg, 2: -180--90 deg, 3: -90--0 deg
     uint8_t turn_state;
     if (ahrs.get_yaw() >= 0.0f && ahrs.get_yaw() < radians(90)) {
         turn_state = 0;
-    } else if (ahrs.get_yaw() > radians(90)) {
+    } else if (ahrs.get_yaw() >= radians(90)) {
         turn_state = 1;
     } else if (ahrs.get_yaw() < -radians(90)) {
         turn_state = 2;
@@ -19,39 +19,10 @@ void Sub::update_turn_counter()
     }
 
     // If yaw went from negative to positive (right turn)
-    switch (last_turn_state) {
-    case 0:
-        if (turn_state == 1) {
-            quarter_turn_count++;
-        }
-        if (turn_state == 3) {
-            quarter_turn_count--;
-        }
-        break;
-    case 1:
-        if (turn_state == 2) {
-            quarter_turn_count++;
-        }
-        if (turn_state == 0) {
-            quarter_turn_count--;
-        }
-        break;
-    case 2:
-        if (turn_state == 3) {
-            quarter_turn_count++;
-        }
-        if (turn_state == 1) {
-            quarter_turn_count--;
-        }
-        break;
-    case 3:
-        if (turn_state == 0) {
-            quarter_turn_count++;
-        }
-        if (turn_state == 2) {
-            quarter_turn_count--;
-        }
-        break;
+    if (turn_state == (last_turn_state + 1) % 4) {
+        quarter_turn_count++;
+    else if (turn_state == (uint8_t)(last_turn_state - 1) % 4) {
+        quarter_turn_count--;
     }
     last_turn_state = turn_state;
 }