diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index b5a43c1f77..d6fcfb386e 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -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); } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0e5c63c611..b8eaf0fe28 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -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; } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 4404fe9900..cc670718ad 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -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. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 6f1cf881bd..52753d6531 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 43fe3bd312..79960f13cb 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 000fa32e86..58b6fa1a14 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -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) diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 61fd36410d..9a69b34de9 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -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; diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index b9085e7889..27cca5d0a4 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -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; } /*