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

Plane: take advantage of L1 stale flag and correctly inhibit flight stage if not lined up on approach

This commit is contained in:
Tom Pittenger 2016-04-18 09:37:30 -07:00
parent 25c3367341
commit 183369b92a
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.cpp
View File

@ -927,9 +927,11 @@ void Plane::update_flight_stage(void)
} else if (auto_state.land_pre_flare == true) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
} else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
bool lined_up = abs(nav_controller->bearing_error_cd()) < 1000;
bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale();
bool on_flight_line = abs(nav_controller->crosstrack_error() < 5) && !nav_controller->data_is_stale();
bool below_prev_WP = current_loc.alt < prev_WP_loc.alt;
if ((auto_state.wp_proportion > 0.15 && lined_up && below_prev_WP) ||
if ((auto_state.wp_proportion >= 0 && heading_lined_up && on_flight_line) ||
(auto_state.wp_proportion > 0.15f && heading_lined_up && below_prev_WP) ||
(auto_state.wp_proportion > 0.5f)) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
} else {

2
ArduPlane/commands_logic.cpp
View File

@ -32,6 +32,8 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
// once landed, post some landing statistics to the GCS
auto_state.post_landing_stats = false;
nav_controller->set_data_is_stale();
// reset loiter start time. New command is a new loiter
loiter.start_time_ms = 0;