Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add AUTO RTL pseudo mode, DO_LAND_START

Browse Source
This commit is contained in:
Iampete1 2021-07-25 00:25:18 +01:00 committed by Randy Mackay
parent 7d3fa242d7
commit b63cc809ce
5 changed files with 61 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/Parameters.cpp
View File

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

8
ArduCopter/RC_Channel.cpp
View File

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

7
ArduCopter/mode.cpp
View File

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

13
ArduCopter/mode.h
View File

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

31
ArduCopter/mode_auto.cpp
View File

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