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

Tracker: correct scan mode pitch clamping

This commit is contained in:
Peter Barker 2019-12-03 23:55:58 +11:00 committed by Randy Mackay
parent 1c3287160e
commit a8e2923774
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
AntennaTracker/mode.cpp
View File

@ -45,15 +45,19 @@ void Mode::update_scan(void)
}
if (!nav_status.manual_control_pitch) {
float pitch_delta = g.scan_speed_pitch * 0.02f;
nav_status.pitch += pitch_delta * (nav_status.scan_reverse_pitch?-1:1);
if (nav_status.pitch < g.pitch_min && nav_status.scan_reverse_pitch) {
nav_status.scan_reverse_pitch = false;
const float pitch_delta = g.scan_speed_pitch * 0.02f;
if (nav_status.scan_reverse_pitch) {
nav_status.pitch -= pitch_delta;
if (nav_status.pitch < g.pitch_min) {
nav_status.scan_reverse_pitch = false;
}
} else {
nav_status.pitch += pitch_delta;
if (nav_status.pitch > g.pitch_max) {
nav_status.scan_reverse_pitch = true;
}
}
if (nav_status.pitch > g.pitch_max && !nav_status.scan_reverse_pitch) {
nav_status.scan_reverse_pitch = true;
}
nav_status.pitch = constrain_float(nav_status.pitch, -g.pitch_min, g.pitch_max);
nav_status.pitch = constrain_float(nav_status.pitch, g.pitch_min, g.pitch_max);
}
update_auto();