mirror of https://github.com/ArduPilot/ardupilot
Plane: added flight stage FLIGHT_LAND_ABORT and abort mechanism
- enabled via new param LAND_ABORT_THR default is 0 (disabled) - Triggered via 95% throttle during landing, a landing abort will take place. - This copies all takeoff params for right now, we can make this better later if needed - added mission item command to NAV_LAND which is the abort takeoff altitude. If 0 then use last takeoff if available, else use 30m - hold heading, just like takeoff, until altitude is reached - pitch is constrained to takeoff pitch, or else 10deg if not available - After abort altitude is reached, the normal landing restart happens (DO_LAND_START or decrement mission) - restart landing by jumping to DO_LAND_START or decrement mission on mode change
This commit is contained in:
parent
cb20325593
commit
25da4ec0ea
|
@ -494,15 +494,12 @@ void Plane::handle_auto_mode(void)
|
|||
nav_cmd_id = auto_rtl_command.id;
|
||||
}
|
||||
|
||||
switch(nav_cmd_id) {
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
|
||||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
|
||||
takeoff_calc_roll();
|
||||
takeoff_calc_pitch();
|
||||
calc_throttle();
|
||||
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_LAND:
|
||||
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
|
||||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
|
||||
|
@ -518,9 +515,7 @@ void Plane::handle_auto_mode(void)
|
|||
// zero throttle
|
||||
channel_throttle->servo_out = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
} else {
|
||||
// we are doing normal AUTO flight, the special cases
|
||||
// are for takeoff and landing
|
||||
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
|
||||
|
@ -530,7 +525,6 @@ void Plane::handle_auto_mode(void)
|
|||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
calc_throttle();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -776,10 +770,12 @@ void Plane::update_navigation()
|
|||
*/
|
||||
void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
|
||||
{
|
||||
//if just now entering land flight stage
|
||||
if (fs == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
|
||||
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
|
||||
if (fs == flight_stage) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (fs) {
|
||||
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
|
||||
#if GEOFENCE_ENABLED == ENABLED
|
||||
if (g.fence_autoenable == 1) {
|
||||
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
|
||||
|
@ -795,8 +791,19 @@ void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
|
|||
}
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
|
||||
gcs_send_text_fmt(PSTR("Landing aborted via throttle, climbing to %dm"), auto_state.takeoff_altitude_rel_cm/100);
|
||||
break;
|
||||
|
||||
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
|
||||
case AP_SpdHgtControl::FLIGHT_NORMAL:
|
||||
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
flight_stage = fs;
|
||||
}
|
||||
|
||||
|
@ -836,11 +843,18 @@ void Plane::update_flight_stage(void)
|
|||
if (control_mode==AUTO) {
|
||||
if (auto_state.takeoff_complete == false) {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
|
||||
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND &&
|
||||
auto_state.land_complete == true) {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
|
||||
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
|
||||
|
||||
if ((g.land_abort_throttle_enable && channel_throttle->control_in > 95) ||
|
||||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
|
||||
// abort mode is sticky, it must complete while executing NAV_LAND
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);
|
||||
} else if (auto_state.land_complete == true) {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
|
||||
} else {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
|
||||
}
|
||||
|
||||
} else {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
|
||||
}
|
||||
|
|
|
@ -444,7 +444,8 @@ void Plane::calc_nav_yaw_ground(void)
|
|||
{
|
||||
if (gps.ground_speed() < 1 &&
|
||||
channel_throttle->control_in == 0 &&
|
||||
flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF) {
|
||||
flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF &&
|
||||
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
|
||||
// manual rudder control while still
|
||||
steer_state.locked_course = false;
|
||||
steer_state.locked_course_err = 0;
|
||||
|
@ -453,7 +454,8 @@ void Plane::calc_nav_yaw_ground(void)
|
|||
}
|
||||
|
||||
float steer_rate = (rudder_input/4500.0f) * g.ground_steer_dps;
|
||||
if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
|
||||
if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF ||
|
||||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
|
||||
steer_rate = 0;
|
||||
}
|
||||
if (!is_zero(steer_rate)) {
|
||||
|
@ -462,7 +464,8 @@ void Plane::calc_nav_yaw_ground(void)
|
|||
} else if (!steer_state.locked_course) {
|
||||
// pilot has released the rudder stick or we are still - lock the course
|
||||
steer_state.locked_course = true;
|
||||
if (flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF) {
|
||||
if (flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF &&
|
||||
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
|
||||
steer_state.locked_course_err = 0;
|
||||
}
|
||||
}
|
||||
|
@ -846,7 +849,8 @@ void Plane::set_servos(void)
|
|||
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
|
||||
min_throttle = 0;
|
||||
}
|
||||
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
|
||||
if (control_mode == AUTO &&
|
||||
(flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
|
||||
if(aparm.takeoff_throttle_max != 0) {
|
||||
max_throttle = aparm.takeoff_throttle_max;
|
||||
} else {
|
||||
|
@ -923,6 +927,7 @@ void Plane::set_servos(void)
|
|||
if (control_mode == AUTO) {
|
||||
switch (flight_stage) {
|
||||
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
|
||||
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
|
||||
if (g.takeoff_flap_percent != 0) {
|
||||
auto_flap_percent = g.takeoff_flap_percent;
|
||||
}
|
||||
|
|
|
@ -260,6 +260,14 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
|
|||
// @User: Advanced
|
||||
GSCALAR(land_disarm_delay, "LAND_DISARMDELAY", 20),
|
||||
|
||||
// @Param: LAND_ABORT_THR
|
||||
// @DisplayName: Landing abort using throttle
|
||||
// @Description: Allow a landing abort to trigger with a throttle > 95%
|
||||
// @Units: boolean
|
||||
// @Range: 0 1
|
||||
// @User: Advanced
|
||||
GSCALAR(land_abort_throttle_enable, "LAND_ABORT_THR", 0),
|
||||
|
||||
// @Param: NAV_CONTROLLER
|
||||
// @DisplayName: Navigation controller selection
|
||||
// @Description: Which navigation controller to enable. Currently the only navigation controller available is L1. From time to time other experimental controllers will be added which are selected using this parameter.
|
||||
|
|
|
@ -140,6 +140,7 @@ public:
|
|||
k_param_gcs3, // 93
|
||||
k_param_gcs_pid_mask,
|
||||
k_param_crash_detection_enable,
|
||||
k_param_land_abort_throttle_enable,
|
||||
|
||||
// 97: RSSI
|
||||
k_param_rssi = 97,
|
||||
|
@ -449,6 +450,7 @@ public:
|
|||
AP_Int32 RTL_altitude_cm;
|
||||
AP_Float land_flare_alt;
|
||||
AP_Int8 land_disarm_delay;
|
||||
AP_Int8 land_abort_throttle_enable;
|
||||
AP_Int32 min_gndspeed_cm;
|
||||
AP_Int16 pitch_trim_cd;
|
||||
AP_Int16 FBWB_min_altitude_cm;
|
||||
|
|
|
@ -947,7 +947,7 @@ private:
|
|||
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
|
||||
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
|
||||
void run_cli(AP_HAL::UARTDriver *port);
|
||||
void restart_landing_sequence();
|
||||
bool restart_landing_sequence();
|
||||
void log_init();
|
||||
uint32_t millis() const;
|
||||
uint32_t micros() const;
|
||||
|
|
|
@ -329,7 +329,6 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
// zero locked course
|
||||
steer_state.locked_course_err = 0;
|
||||
|
||||
}
|
||||
|
||||
void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
|
@ -341,6 +340,19 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
|
|||
{
|
||||
auto_state.commanded_go_around = false;
|
||||
set_next_WP(cmd.content.location);
|
||||
|
||||
// configure abort altitude and pitch
|
||||
// if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m
|
||||
if (cmd.p1 > 0) {
|
||||
auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100;
|
||||
} else if (auto_state.takeoff_altitude_rel_cm <= 0) {
|
||||
auto_state.takeoff_altitude_rel_cm = 3000;
|
||||
}
|
||||
|
||||
if (auto_state.takeoff_pitch_cd <= 0) {
|
||||
// If no takeoff command has ever been used, default to a conservative 10deg
|
||||
auto_state.takeoff_pitch_cd = 1000;
|
||||
}
|
||||
}
|
||||
|
||||
void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
|
||||
|
|
|
@ -57,6 +57,7 @@ void Plane::update_is_flying_5Hz(void)
|
|||
}
|
||||
break;
|
||||
|
||||
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
|
||||
case AP_SpdHgtControl::FLIGHT_NORMAL:
|
||||
// TODO: detect ground impacts
|
||||
break;
|
||||
|
@ -162,6 +163,7 @@ void Plane::crash_detection_update(void)
|
|||
}
|
||||
break;
|
||||
|
||||
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
|
||||
case AP_SpdHgtControl::FLIGHT_NORMAL:
|
||||
if (been_auto_flying) {
|
||||
crashed = true;
|
||||
|
|
|
@ -25,6 +25,29 @@ bool Plane::verify_land()
|
|||
return true;
|
||||
}
|
||||
|
||||
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
|
||||
// the altitude has been reached, restart the landing sequence
|
||||
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
|
||||
|
||||
throttle_suppressed = false;
|
||||
auto_state.land_complete = false;
|
||||
nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
|
||||
|
||||
// see if we have reached abort altitude
|
||||
if (adjusted_relative_altitude_cm() > auto_state.takeoff_altitude_rel_cm) {
|
||||
next_WP_loc = current_loc;
|
||||
mission.stop();
|
||||
bool success = restart_landing_sequence();
|
||||
mission.resume();
|
||||
if (!success) {
|
||||
// on a restart failure lets RTL or else the plane may fly away with nowhere to go!
|
||||
set_mode(RTL);
|
||||
}
|
||||
// make sure to return false so it leaves the mission index alone
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
float height = height_above_target();
|
||||
|
||||
// use rangefinder to correct if possible
|
||||
|
@ -95,7 +118,8 @@ bool Plane::verify_land()
|
|||
we return false as a landing mission item never completes
|
||||
|
||||
we stay on this waypoint unless the GCS commands us to change
|
||||
mission item or reset the mission, or a go-around is commanded
|
||||
mission item, reset the mission, command a go-around or finish
|
||||
a land_abort procedure.
|
||||
*/
|
||||
return false;
|
||||
}
|
||||
|
@ -216,28 +240,34 @@ void Plane::setup_landing_glide_slope(void)
|
|||
jump there. Otherwise decrement waypoint so we would re-start
|
||||
from the top with same glide slope. Return true if successful.
|
||||
*/
|
||||
void Plane::restart_landing_sequence()
|
||||
bool Plane::restart_landing_sequence()
|
||||
{
|
||||
if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) {
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
uint16_t do_land_start_index = mission.get_landing_sequence_start();
|
||||
uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index();
|
||||
bool success = false;
|
||||
|
||||
if (do_land_start_index != 0 &&
|
||||
mission.set_current_cmd(do_land_start_index))
|
||||
{
|
||||
// look for a DO_LAND_START and use that index
|
||||
gcs_send_text_fmt(PSTR("Restarted landing via DO_LAND_START: %d"),do_land_start_index);
|
||||
success = true;
|
||||
} else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
|
||||
mission.set_current_cmd(prev_cmd_with_wp_index))
|
||||
{
|
||||
// if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then
|
||||
// repeat that cmd to restart the landing from the top of approach to repeat intended glide slope
|
||||
gcs_send_text_fmt(PSTR("Restarted landing sequence at waypoint %d"), prev_cmd_with_wp_index);
|
||||
success = true;
|
||||
} else {
|
||||
gcs_send_text_fmt(PSTR("Unable to restart landing sequence!"));
|
||||
success = false;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
Loading…
Reference in New Issue