Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Turn counter (#23) 

* Sub: Add turn counter to avoid tether tangling.

* Sub: Add turn counter.

* Sub: Bug fix in turn counter.
This commit is contained in:
Rustom Jehangir 2016-07-03 20:29:34 -04:00 committed by Andrew Tridgell
parent d3f5f59b8c
commit accbbce304
3 changed files with 69 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/ArduSub.cpp
View File

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

5
ArduSub/Sub.h
View File

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

63
ArduSub/turn_counter.cpp Normal file
View File

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