From 2c62158e2045d03238b2ab0e77f2e02c2b59ae57 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Mon, 10 Oct 2016 22:59:10 -0400 Subject: [PATCH] Sub: turn counter improvements --- ArduSub/turn_counter.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduSub/turn_counter.cpp b/ArduSub/turn_counter.cpp index 8c4ffe5822..61485056e1 100644 --- a/ArduSub/turn_counter.cpp +++ b/ArduSub/turn_counter.cpp @@ -55,8 +55,11 @@ void Sub::update_turn_counter() break; } static int32_t last_turn_count_printed; - if ( quarter_turn_count/4 != last_turn_count_printed ) { - gcs_send_text_fmt(MAV_SEVERITY_INFO,"Tether is turned %i turns %s",int32_t(quarter_turn_count/4),(quarter_turn_count>0)?"to the right":"to the left"); + static uint32_t last_turn_announce_ms = 0; + uint32_t tnow = AP_HAL::millis(); + if ( quarter_turn_count/4 != last_turn_count_printed && tnow > last_turn_announce_ms + 2000) { + last_turn_announce_ms = tnow; + gcs_send_text_fmt(MAV_SEVERITY_INFO,"Tether is turned %i turns %s",int32_t(abs(quarter_turn_count)/4),(quarter_turn_count>0)?"to the right":"to the left"); last_turn_count_printed = quarter_turn_count/4; } last_turn_state = turn_state;