diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index c35d96e4ba52c1..3022143aee77f0 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -841,6 +841,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i return MAV_RESULT_ACCEPTED; #if MODE_AUTO_ENABLED == ENABLED + case MAV_CMD_DO_LAND_REJOIN: + if (copter.mode_auto.rejoin_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + case MAV_CMD_DO_LAND_START: if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 921f9fb32902e6..acf25233dd2606 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -276,7 +276,8 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) #if MODE_AUTO_ENABLED == ENABLED if (mode == Mode::Number::AUTO_RTL) { // Special case for AUTO RTL, not a true mode, just AUTO in disguise - return mode_auto.jump_to_landing_sequence_auto_RTL(reason); + // Attempt to rejoin, fallback to do-land-start + return mode_auto.rejoin_or_jump_to_landing_sequence_auto_RTL(reason); } #endif @@ -1057,3 +1058,12 @@ uint16_t Mode::get_pilot_speed_dn() { return copter.get_pilot_speed_dn(); } + +// Return stopping point as a location with above origin alt frame +Location Mode::get_stopping_point() const +{ + Vector3p stopping_point_NEU; + copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy()); + copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z); + return Location { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN }; +} diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dd06ee0149c296..f61ca84d45f185 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -185,6 +185,9 @@ class Mode { virtual bool allows_weathervaning() const { return false; } #endif + // Return stopping point as a location with above origin alt frame + Location get_stopping_point() const; + protected: // helper functions @@ -544,6 +547,12 @@ class ModeAuto : public Mode { // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode bool jump_to_landing_sequence_auto_RTL(ModeReason reason); + // Rejoin mission after DO_LAND_REJOIN waypoint, if succeeds pretend to be Auto RTL mode + bool rejoin_landing_sequence_auto_RTL(ModeReason reason); + + // Try rejoin else do land start + bool rejoin_or_jump_to_landing_sequence_auto_RTL(ModeReason reason); + // lua accessors for nav script time support bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4); void nav_script_time_done(uint16_t id); @@ -580,6 +589,9 @@ class ModeAuto : public Mode { AllowWeatherVaning = (1 << 7U), }; + // Enter auto rtl pseudo mode + bool enter_auto_rtl(ModeReason reason); + bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); void exit_mission(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 3dc925229f8472..0d9fb937c69c9a 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -219,35 +219,79 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) { // Use stopping point as location for start of auto RTL - Vector3p stopping_point_NEU; - copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy()); - copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z); - Location stopping_point { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN }; - - if (mission.jump_to_landing_sequence(stopping_point)) { - mission.set_force_resume(true); - // if not already in auto switch to auto - if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) { - auto_RTL = true; + Location stopping_point = get_stopping_point(); + if (!mission.jump_to_landing_sequence(stopping_point)) { + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL)); + // make sad noise + if (copter.ap.initialised) { + AP_Notify::events.user_mode_change_failed = 1; + } + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found"); + return false; + } + + return enter_auto_rtl(reason); +} + +// Rejoin mission after DO_LAND_REJOIN waypoint, if succeeds pretend to be Auto RTL mode +bool ModeAuto::rejoin_landing_sequence_auto_RTL(ModeReason reason) +{ + // Use stopping point as location for start of auto RTL + Location stopping_point = get_stopping_point(); + if (!mission.jump_to_closest_mission_leg(stopping_point)) { + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL)); + // make sad noise + if (copter.ap.initialised) { + AP_Notify::events.user_mode_change_failed = 1; + } + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No rejoin found"); + return false; + } + + return enter_auto_rtl(reason); +} + +// Try rejoin else do land start +bool ModeAuto::rejoin_or_jump_to_landing_sequence_auto_RTL(ModeReason reason) +{ + // Use stopping point as location for start of auto RTL + Location stopping_point = get_stopping_point(); + if (!mission.jump_to_closest_mission_leg(stopping_point) && !mission.jump_to_landing_sequence(stopping_point)) { + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL)); + // make sad noise + if (copter.ap.initialised) { + AP_Notify::events.user_mode_change_failed = 1; + } + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No rejoin or landing sequence found"); + return false; + } + + return enter_auto_rtl(reason); +} + +// Enter auto rtl pseudo mode +bool ModeAuto::enter_auto_rtl(ModeReason reason) +{ + mission.set_force_resume(true); + + // if not already in auto switch to auto + if ((copter.flightmode == this) || set_mode(Mode::Number::AUTO, reason)) { + auto_RTL = true; #if HAL_LOGGING_ENABLED - // log entry into AUTO RTL - copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason); + // log entry into AUTO RTL + copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason); #endif - // make happy noise - if (copter.ap.initialised) { - AP_Notify::events.user_mode_change = 1; - } - return true; + // make happy noise + if (copter.ap.initialised) { + AP_Notify::events.user_mode_change = 1; } - // mode change failed, revert force resume flag - mission.set_force_resume(false); - - gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed"); - } else { - gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found"); + return true; } + // mode change failed, revert force resume flag + mission.set_force_resume(false); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL)); // make sad noise if (copter.ap.initialised) { diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index b8a6cd9bbe7af5..d3250e4265642f 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -387,10 +387,7 @@ void ModeRTL::land_run(bool disarm_on_land) void ModeRTL::build_path() { // origin point is our stopping point - Vector3p stopping_point; - pos_control->get_stopping_point_xy_cm(stopping_point.xy()); - pos_control->get_stopping_point_z_cm(stopping_point.z); - rtl_path.origin_point = Location(stopping_point.tofloat(), Location::AltFrame::ABOVE_ORIGIN); + rtl_path.origin_point = get_stopping_point(); rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME); // compute return target