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

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:
Tom Pittenger 2015-05-09 09:36:40 -07:00 committed by Andrew Tridgell
parent cb20325593
commit 25da4ec0ea
8 changed files with 100 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
ArduPlane/ArduPlane.cpp
View File

@ -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) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
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);
}

13
ArduPlane/Attitude.cpp
View File

@ -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;
}

8
ArduPlane/Parameters.cpp
View File

@ -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.

2
ArduPlane/Parameters.h
View File

@ -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;

2
ArduPlane/Plane.h
View File

@ -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;

14
ArduPlane/commands_logic.cpp
View File

@ -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)

2
ArduPlane/is_flying.cpp
View File

@ -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;

38
ArduPlane/landing.cpp
View File

@ -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;
}
/*