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

@@ -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

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);

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;
+}