mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-02-06 07:42:54 +08:00
29 lines
843 B
C++
29 lines
843 B
C++
// 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.get_yaw_rad() >= 0.0f && ahrs.get_yaw_rad() < radians(90)) {
|
|
turn_state = 0;
|
|
} else if (ahrs.get_yaw_rad() >= radians(90)) {
|
|
turn_state = 1;
|
|
} else if (ahrs.get_yaw_rad() < -radians(90)) {
|
|
turn_state = 2;
|
|
} else {
|
|
turn_state = 3;
|
|
}
|
|
|
|
// If yaw went from negative to positive (right turn)
|
|
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;
|
|
}
|