aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: add flight stage LAND_PREFLARE

Browse Source
This commit is contained in:
Tom Pittenger 2016-01-29 22:32:42 -08:00 committed by Andrew Tridgell
parent 4bc30f6479
commit b8d5369ebd
9 changed files with 28 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/ArduPlane.cpp
View File

@ -844,6 +844,7 @@ void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted via throttle. Climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
case AP_SpdHgtControl::FLIGHT_NORMAL:
case AP_SpdHgtControl::FLIGHT_VTOL:

5
ArduPlane/Attitude.cpp
View File

@ -217,7 +217,9 @@ void Plane::stabilize_yaw(float speed_scaler)
// are below the GROUND_STEER_ALT
steering_control.ground_steering = (channel_roll->control_in == 0 &&
fabsf(relative_altitude()) < g.ground_steer_alt);
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
if (control_mode == AUTO &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE)) {
// don't use ground steering on landing approach
steering_control.ground_steering = false;
}
@ -987,6 +989,7 @@ void Plane::set_servos(void)
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
if (g.land_flap_percent != 0) {
auto_flap_percent = g.land_flap_percent;

4
ArduPlane/GCS_Mavlink.cpp
View File

@ -1390,7 +1390,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
//Not allowing go around at FLIGHT_LAND_FINAL stage on purpose --
//if plane is close to the ground a go around coudld be dangerous.
if (plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
if (plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
// Initiate an aborted landing. This will trigger a pitch-up and
// climb-out to a safe altitude holding heading then one of the
// following actions will occur, check for in this order:

11
ArduPlane/altitude.cpp
View File

@ -34,7 +34,8 @@ void Plane::adjust_altitude_target()
// in land final TECS uses TECS_LAND_SINK as a target sink
// rate, and ignores the target altitude
set_target_altitude_location(next_WP_loc);
} else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
} else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) {
setup_landing_glide_slope();
} else if (nav_controller->reached_loiter_target()) {
// once we reach a loiter target then lock to the final
@ -357,7 +358,8 @@ void Plane::set_offset_altitude_location(const Location &loc)
}
#endif
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
// if we are within GLIDE_SLOPE_MIN meters of the target altitude
// then reset the offset to not use a glide slope. This allows for
@ -536,6 +538,7 @@ float Plane::rangefinder_correction(void)
bool using_rangefinder = (g.rangefinder_landing &&
control_mode == AUTO &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL));
if (!using_rangefinder) {
return 0;
@ -576,7 +579,9 @@ void Plane::rangefinder_height_update(void)
} else {
rangefinder_state.in_range = true;
if (!rangefinder_state.in_use &&
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) &&
g.rangefinder_landing) {
rangefinder_state.in_use = true;
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)height_estimate);

1
ArduPlane/events.cpp
View File

@ -138,6 +138,7 @@ void Plane::low_battery_event(void)
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh",
(double)battery.voltage(), (double)battery.current_total_mah());
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
set_mode(RTL);
aparm.throttle_cruise.load();

2
ArduPlane/is_flying.cpp
View File

@ -93,6 +93,7 @@ void Plane::update_is_flying_5Hz(void)
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
break;
@ -218,6 +219,7 @@ void Plane::crash_detection_update(void)
// a crash into a tree would be caught here.
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
// We should be nice and level-ish in this flight stage. If not, we most
// likely had a crazy landing. Throttle is inhibited already at the flare

1
ArduPlane/landing.cpp
View File

@ -314,6 +314,7 @@ float Plane::tecs_hgt_afe(void)
*/
float hgt_afe;
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
hgt_afe = height_above_target();
hgt_afe -= rangefinder_correction();

1
ArduPlane/sensors.cpp
View File

@ -36,6 +36,7 @@ void Plane::read_rangefinder(void)
{
// use the best available alt estimate via baro above home
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
// ensure the rangefinder is powered-on when land alt is higher than home altitude.
// This is done using the target alt which we know is below us and we are sinking to it

10
ArduPlane/system.cpp
View File

@ -528,7 +528,9 @@ void Plane::check_long_failsafe()
uint32_t tnow = millis();
// only act on changes
// -------------------
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
if (failsafe.state == FAILSAFE_SHORT &&
(tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) {
@ -562,8 +564,10 @@ void Plane::check_short_failsafe()
{
// only act on changes
// -------------------
if(failsafe.state == FAILSAFE_NONE && (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH)) {
if(failsafe.state == FAILSAFE_NONE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
if(failsafe.ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde
failsafe_short_on_event(FAILSAFE_SHORT);
}