mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Copter: add AUTO RTL pseudo mode, DO_LAND_START
This commit is contained in:
parent
7d3fa242d7
commit
b63cc809ce
@ -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),
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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");
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user