From accbbce304f7dcb784bd2d06ec7a31bdec17be2c Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Sun, 3 Jul 2016 20:29:34 -0400 Subject: [PATCH] Sub: Turn counter (#23) * Sub: Add turn counter to avoid tether tangling. * Sub: Add turn counter. * Sub: Bug fix in turn counter. --- ArduSub/ArduSub.cpp | 1 + ArduSub/Sub.h | 5 ++++ ArduSub/turn_counter.cpp | 63 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 69 insertions(+) create mode 100644 ArduSub/turn_counter.cpp diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 6788100605..2f9ee4fb4f 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -99,6 +99,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(run_nav_updates, 50, 100), SCHED_TASK(update_thr_average, 100, 90), SCHED_TASK(three_hz_loop, 3, 75), + SCHED_TASK(update_turn_counter, 10, 50), SCHED_TASK(compass_accumulate, 100, 100), SCHED_TASK(barometer_accumulate, 50, 90), #if PRECISION_LANDING == ENABLED diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index a79b7ff7af..3380936111 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -429,6 +429,10 @@ private: float baro_climbrate; // barometer climbrate in cm/s LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests + // Turn counter + int32_t quarter_turn_count; + uint8_t last_turn_state; + // filtered pilot's throttle input used to cancel landing if throttle held high LowPassFilterFloat rc_throttle_control_in_filter; @@ -588,6 +592,7 @@ private: void three_hz_loop(); void one_hz_loop(); void update_GPS(void); + void update_turn_counter(); void init_simple_bearing(); void update_simple_mode(void); void update_super_simple_bearing(bool force_update); diff --git a/ArduSub/turn_counter.cpp b/ArduSub/turn_counter.cpp new file mode 100644 index 0000000000..8c4ffe5822 --- /dev/null +++ b/ArduSub/turn_counter.cpp @@ -0,0 +1,63 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// Code by Rustom Jehangir: rusty@bluerobotics.com + +#include "Sub.h" + +// Count total vehicle turns to avoid tangling tether +void Sub::update_turn_counter() +{ + // Determine state + // 0: 0-90 deg, 1: 90-180 deg, 2: -180--90 deg, 3: -90--0 deg + uint8_t turn_state; + if ( ahrs.yaw >= 0.0f && ahrs.yaw < radians(90) ) { + turn_state = 0; + } else if ( ahrs.yaw > radians(90) ) { + turn_state = 1; + } else if ( ahrs.yaw < -radians(90) ) { + turn_state = 2; + } else { + turn_state = 3; + } + + // 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; + } + 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"); + last_turn_count_printed = quarter_turn_count/4; + } + last_turn_state = turn_state; +}