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

Plane: rely on flight stage to know if we're doing auto_land in TECS

This commit is contained in:
Tom Pittenger 2016-04-19 17:11:00 -07:00
parent 595badce3e
commit 85913bd237
1 changed files with 18 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ArduPlane/ArduPlane.cpp
View File

@ -870,25 +870,27 @@ void Plane::update_alt()
update_flight_stage();
bool is_doing_auto_land = (control_mode == AUTO) && (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND);
float distance_beyond_land_wp = 0;
if (is_doing_auto_land && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
if (auto_throttle_mode && !throttle_suppressed) {
// set Flight stage for controller. If not in AUTO then assume normal operation.
// this prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during an aborted landing
AP_SpdHgtControl::FlightStage fs = flight_stage;
if (control_mode != AUTO) {
fs = AP_SpdHgtControl::FLIGHT_NORMAL;
bool is_doing_auto_land = false;
float distance_beyond_land_wp = 0;
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
is_doing_auto_land = true;
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
break;
default:
break;
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
fs,
flight_stage,
is_doing_auto_land,
distance_beyond_land_wp,
auto_state.takeoff_pitch_cd,
@ -940,6 +942,9 @@ void Plane::update_flight_stage(void)
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else {
// If not in AUTO then assume normal operation for normal TECS operation.
// This prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else if (quadplane.in_vtol_mode() ||