From bd567e013b7b3a2ba888afed8990fe9c1b8335d6 Mon Sep 17 00:00:00 2001 From: ES-Alexander Date: Thu, 8 May 2025 17:34:01 +1000 Subject: [PATCH] ArduSub: improve turn counting --- ArduSub/GCS_MAVLink_Sub.cpp | 2 +- ArduSub/turn_counter.cpp | 41 ++++++------------------------------- 2 files changed, 7 insertions(+), 36 deletions(-) diff --git a/ArduSub/GCS_MAVLink_Sub.cpp b/ArduSub/GCS_MAVLink_Sub.cpp index 3db7c60afc6f94..8e958f657e551b 100644 --- a/ArduSub/GCS_MAVLink_Sub.cpp +++ b/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", diff --git a/ArduSub/turn_counter.cpp b/ArduSub/turn_counter.cpp index 52df84fecb7486..09d7cd9599f509 100644 --- a/ArduSub/turn_counter.cpp +++ b/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; }