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

Plane: Cruise: only lock in heading once moving forwards

This commit is contained in:
Iampete1 2023-11-01 14:56:54 +00:00 committed by Andrew Tridgell
parent 369f369f1d
commit 55040175e6
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/mode_cruise.cpp
View File

@ -58,11 +58,17 @@ void ModeCruise::navigate()
return;
}
#endif
// check if we are moving in the direction of the front of the vehicle
const int32_t ground_course_cd = plane.gps.ground_course_cd();
const bool moving_forwards = fabsf(wrap_PI(radians(ground_course_cd * 0.01) - plane.ahrs.yaw)) < M_PI_2;
if (!locked_heading &&
plane.channel_roll->get_control_in() == 0 &&
plane.rudder_input() == 0 &&
plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
plane.gps.ground_speed() >= 3 &&
moving_forwards &&
lock_timer_ms == 0) {
// user wants to lock the heading - start the timer
lock_timer_ms = millis();
@ -73,7 +79,7 @@ void ModeCruise::navigate()
// from user
locked_heading = true;
lock_timer_ms = 0;
locked_heading_cd = plane.gps.ground_course_cd();
locked_heading_cd = ground_course_cd;
plane.prev_WP_loc = plane.current_loc;
}
if (locked_heading) {