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

Plane: FW approach: correct wrap on breakout direction check

This commit is contained in:
Iampete1 2022-07-15 00:28:56 +01:00 committed by Andrew Tridgell
parent 2e342d7852
commit 2bda3c44c6
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/commands_logic.cpp
View File

@ -1055,10 +1055,10 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
{
nav_controller->update_loiter(cmd.content.location, radius, direction);
const float breakout_direction_rad = radians(wrap_180(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90)));
const float breakout_direction_rad = radians(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90));
// breakout when within 5 degrees of the opposite direction
if (fabsf(ahrs.yaw - breakout_direction_rad) < radians(5.0f)) {
if (fabsf(wrap_PI(ahrs.yaw - breakout_direction_rad)) < radians(5.0f)) {
gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path");
vtol_approach_s.approach_stage = APPROACH_LINE;
set_next_WP(cmd.content.location);