diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 074ab5aac6..811311d316 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -269,42 +269,42 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FLTMODE1 // @DisplayName: Flight Mode 1 // @Description: Flight mode when Channel 5 pwm is <= 1230 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate,27:Auto RTL // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1), // @Param: FLTMODE2 // @DisplayName: Flight Mode 2 // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate,27:Auto RTL // @User: Standard GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2), // @Param: FLTMODE3 // @DisplayName: Flight Mode 3 // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate,27:Auto RTL // @User: Standard GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3), // @Param: FLTMODE4 // @DisplayName: Flight Mode 4 // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate,27:Auto RTL // @User: Standard GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4), // @Param: FLTMODE5 // @DisplayName: Flight Mode 5 // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate,27:Auto RTL // @User: Standard GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5), // @Param: FLTMODE6 // @DisplayName: Flight Mode 6 // @Description: Flight mode when Channel 5 pwm is >=1750 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate,27:Auto RTL // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6), diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 8ffaa17904..066653ca6c 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -98,6 +98,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const AuxS case AUX_FUNC::ZIGZAG_Auto: case AUX_FUNC::ZIGZAG_SaveWP: case AUX_FUNC::ACRO: + case AUX_FUNC::AUTO_RTL: break; case AUX_FUNC::ACRO_TRAINER: case AUX_FUNC::ATTCON_ACCEL_LIM: @@ -578,6 +579,13 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi #endif break; + case AUX_FUNC::AUTO_RTL: +#if MODE_AUTO_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::AUTO_RTL, ch_flag); +#endif + break; + + default: return RC_Channel::do_aux_function(ch_option, ch_flag); } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index fe2fb0cdd8..2aa1265c21 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -197,6 +197,13 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) return true; } +#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); + } +#endif + Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode); if (new_flightmode == nullptr) { gcs().send_text(MAV_SEVERITY_WARNING,"No such mode"); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 373e1d9e7f..05f209d247 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -36,6 +36,7 @@ public: ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers AUTOROTATE = 26, // Autonomous autorotation + AUTO_RTL = 27, // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence }; // constructor @@ -352,7 +353,7 @@ class ModeAuto : public Mode { public: // inherit constructor using Mode::Mode; - Number mode_number() const override { return Number::AUTO; } + Number mode_number() const override { return auto_RTL? Number::AUTO_RTL : Number::AUTO; } bool init(bool ignore_checks) override; void exit() override; @@ -406,6 +407,9 @@ public: // for GCS_MAVLink to call: bool do_guided(const AP_Mission::Mission_Command& cmd); + // 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); + AP_Mission mission{ FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &), @@ -413,8 +417,8 @@ public: protected: - const char *name() const override { return "AUTO"; } - const char *name4() const override { return "AUTO"; } + const char *name() const override { return auto_RTL? "AUTO RTL" : "AUTO"; } + const char *name4() const override { return auto_RTL? "ARTL" : "AUTO"; } uint32_t wp_distance() const override; int32_t wp_bearing() const override; @@ -555,6 +559,9 @@ private: uint8_t cmd_count; // number of commands in the cmd array AP_Mission::Mission_Command cmd[mis_change_detect_cmd_max]; // local copy of the next few mission commands } mis_change_detect = {}; + + // True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode + bool auto_RTL; }; #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index e1ca19b978..e4f56cd147 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -22,6 +22,7 @@ // auto_init - initialise auto controller bool ModeAuto::init(bool ignore_checks) { + auto_RTL = false; if (mission.num_commands() > 1 || ignore_checks) { _mode = SubMode::LOITER; @@ -64,6 +65,8 @@ void ModeAuto::exit() #if HAL_MOUNT_ENABLED copter.camera_mount.set_mode_to_default(); #endif // HAL_MOUNT_ENABLED + + auto_RTL = false; } // auto_run - runs the auto controller @@ -141,13 +144,35 @@ void ModeAuto::run() payload_place_run(); break; } + + // only pretend to be in auto RTL so long as mission still thinks its in a landing sequence or the mission has completed + if (!(mission.get_in_landing_sequence_flag() || mission.state() == AP_Mission::mission_state::MISSION_COMPLETE)) { + auto_RTL = false; + } } bool ModeAuto::allows_arming(AP_Arming::Method method) const { - return (copter.g2.auto_options & (uint32_t)Options::AllowArming) != 0; + return ((copter.g2.auto_options & (uint32_t)Options::AllowArming) != 0) && !auto_RTL; }; +// Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode +bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) +{ + if (mission.jump_to_landing_sequence()) { + mission.set_force_resume(true); + if (set_mode(Mode::Number::AUTO, reason)) { + auto_RTL = true; + return true; + } + // mode change failed, revert force resume flag + mission.set_force_resume(false); + return false; + } + gcs().send_text(MAV_SEVERITY_INFO, "No landing sequence found"); + return false; +} + // auto_loiter_start - initialises loitering in auto mode // returns success/failure because this can be called by exit_mission bool ModeAuto::loiter_start() @@ -528,6 +553,9 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) break; #endif + case MAV_CMD_DO_LAND_START: + break; + default: // unable to use the command, allow the vehicle to try the next command return false; @@ -766,6 +794,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_GUIDED_LIMITS: case MAV_CMD_DO_FENCE_ENABLE: case MAV_CMD_DO_WINCH: + case MAV_CMD_DO_LAND_START: cmd_complete = true; break;