Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Support new AP_Vehicle::set_mode

This commit is contained in:
Michael du Breuil 2019-10-16 20:49:22 -07:00 committed by Randy Mackay
parent 6d7b196212
commit b42b1c08c4
17 changed files with 88 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/Copter.h
View File

@ -388,10 +388,10 @@ private:
// This is the state of the flight control system // This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO, // There are multiple states defined such as STABILIZE, ACRO,
Mode::Number control_mode; Mode::Number control_mode;
mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN; ModeReason control_mode_reason = ModeReason::UNKNOWN;
Mode::Number prev_control_mode; Mode::Number prev_control_mode;
mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN; ModeReason prev_control_mode_reason = ModeReason::UNKNOWN;
RCMapper rcmap; RCMapper rcmap;
@ -719,9 +719,9 @@ private:
void failsafe_terrain_set_status(bool data_ok); void failsafe_terrain_set_status(bool data_ok);
void failsafe_terrain_on_event(); void failsafe_terrain_on_event();
void gpsglitch_check(); void gpsglitch_check();
void set_mode_RTL_or_land_with_pause(mode_reason_t reason); void set_mode_RTL_or_land_with_pause(ModeReason reason);
void set_mode_SmartRTL_or_RTL(mode_reason_t reason); void set_mode_SmartRTL_or_RTL(ModeReason reason);
void set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason); void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
bool should_disarm_on_failsafe(); bool should_disarm_on_failsafe();
// failsafe.cpp // failsafe.cpp
@ -782,12 +782,13 @@ private:
void log_init(void); void log_init(void);
// mode.cpp // mode.cpp
bool set_mode(Mode::Number mode, mode_reason_t reason); bool set_mode(Mode::Number mode, ModeReason reason);
bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
void update_flight_mode(); void update_flight_mode();
void notify_flight_mode(); void notify_flight_mode();
// mode_land.cpp // mode_land.cpp
void set_mode_land_with_pause(mode_reason_t reason); void set_mode_land_with_pause(ModeReason reason);
bool landing_with_GPS(); bool landing_with_GPS();
// motor_test.cpp // motor_test.cpp

33
ArduCopter/GCS_Mavlink.cpp
View File

@ -640,19 +640,19 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
} }
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_GCS_COMMAND)) { if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (!copter.set_mode(Mode::Number::RTL, MODE_REASON_GCS_COMMAND)) { if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
if (!copter.set_mode(Mode::Number::LAND, MODE_REASON_GCS_COMMAND)) { if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
@ -704,7 +704,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
#if MODE_AUTO_ENABLED == ENABLED #if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START: case MAV_CMD_MISSION_START:
if (copter.motors->armed() && if (copter.motors->armed() &&
copter.set_mode(Mode::Number::AUTO, MODE_REASON_GCS_COMMAND)) { copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) {
copter.set_auto_armed(true); copter.set_auto_armed(true);
if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) { if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) {
copter.mode_auto.mission.start_or_resume(); copter.mode_auto.mission.start_or_resume();
@ -800,8 +800,8 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
} }
// set mode to Loiter or fall back to AltHold // set mode to Loiter or fall back to AltHold
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_GCS_COMMAND)) { if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND); copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
} }
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
@ -817,12 +817,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
copter.arming.arm(AP_Arming::Method::MAVLINK); copter.arming.arm(AP_Arming::Method::MAVLINK);
} else if (copter.ap.land_complete) { } else if (copter.ap.land_complete) {
// if armed and landed, takeoff // if armed and landed, takeoff
if (copter.set_mode(Mode::Number::LOITER, MODE_REASON_GCS_COMMAND)) { if (copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
copter.flightmode->do_user_takeoff(packet.param1*100, true); copter.flightmode->do_user_takeoff(packet.param1*100, true);
} }
} else { } else {
// if flying, land // if flying, land
copter.set_mode(Mode::Number::LAND, MODE_REASON_GCS_COMMAND); copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND);
} }
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
@ -844,13 +844,13 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
if (!shot_mode) { if (!shot_mode) {
#if MODE_BRAKE_ENABLED == ENABLED #if MODE_BRAKE_ENABLED == ENABLED
if (copter.set_mode(Mode::Number::BRAKE, MODE_REASON_GCS_COMMAND)) { if (copter.set_mode(Mode::Number::BRAKE, ModeReason::GCS_COMMAND)) {
copter.mode_brake.timeout_to_loiter_ms(2500); copter.mode_brake.timeout_to_loiter_ms(2500);
} else { } else {
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND); copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
} }
#else #else
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND); copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
#endif #endif
} else { } else {
// SoloLink is expected to handle pause in shots // SoloLink is expected to handle pause in shots
@ -1308,17 +1308,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_l
return result; return result;
} }
bool GCS_MAVLINK_Copter::set_mode(const uint8_t mode)
{
#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
if (copter.failsafe.radio) {
// don't allow mode changes while in radio failsafe
return false;
}
#endif
return copter.set_mode((Mode::Number)mode, MODE_REASON_GCS_COMMAND);
}
float GCS_MAVLINK_Copter::vfr_hud_alt() const float GCS_MAVLINK_Copter::vfr_hud_alt() const
{ {
if (copter.g2.dev_options.get() & DevOptionVFR_HUDRelativeAlt) { if (copter.g2.dev_options.get() & DevOptionVFR_HUDRelativeAlt) {

2
ArduCopter/GCS_Mavlink.h
View File

@ -18,8 +18,6 @@ protected:
uint8_t sysid_my_gcs() const override; uint8_t sysid_my_gcs() const override;
bool sysid_enforce() const override; bool sysid_enforce() const override;
bool set_mode(uint8_t mode) override;
bool params_ready() const override; bool params_ready() const override;
void send_banner() override; void send_banner() override;

6
ArduCopter/RC_Channel.cpp
View File

@ -20,7 +20,7 @@ void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos)
return; return;
} }
if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), MODE_REASON_TX_COMMAND)) { if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) {
// alert user to mode change failure // alert user to mode change failure
if (copter.ap.initialised) { if (copter.ap.initialised) {
AP_Notify::events.user_mode_change_failed = 1; AP_Notify::events.user_mode_change_failed = 1;
@ -125,7 +125,7 @@ void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode,
switch(ch_flag) { switch(ch_flag) {
case HIGH: { case HIGH: {
// engage mode (if not possible we remain in current flight mode) // engage mode (if not possible we remain in current flight mode)
const bool success = copter.set_mode(mode, MODE_REASON_TX_COMMAND); const bool success = copter.set_mode(mode, ModeReason::RC_COMMAND);
if (copter.ap.initialised) { if (copter.ap.initialised) {
if (success) { if (success) {
AP_Notify::events.user_mode_change = 1; AP_Notify::events.user_mode_change = 1;
@ -151,7 +151,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
case AUX_FUNC::FLIP: case AUX_FUNC::FLIP:
// flip if switch is on, positive throttle and we're actually flying // flip if switch is on, positive throttle and we're actually flying
if (ch_flag == aux_switch_pos_t::HIGH) { if (ch_flag == aux_switch_pos_t::HIGH) {
copter.set_mode(Mode::Number::FLIP, MODE_REASON_TX_COMMAND); copter.set_mode(Mode::Number::FLIP, ModeReason::RC_COMMAND);
} }
break; break;

14
ArduCopter/avoidance_adsb.cpp
View File

@ -44,7 +44,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O
case MAV_COLLISION_ACTION_RTL: case MAV_COLLISION_ACTION_RTL:
// attempt to switch to RTL, if this fails (i.e. flying in manual mode with bad position) do nothing // attempt to switch to RTL, if this fails (i.e. flying in manual mode with bad position) do nothing
if (failsafe_state_change) { if (failsafe_state_change) {
if (!copter.set_mode(Mode::Number::RTL, MODE_REASON_AVOIDANCE)) { if (!copter.set_mode(Mode::Number::RTL, ModeReason::AVOIDANCE)) {
actual_action = MAV_COLLISION_ACTION_NONE; actual_action = MAV_COLLISION_ACTION_NONE;
} }
} }
@ -53,7 +53,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O
case MAV_COLLISION_ACTION_HOVER: case MAV_COLLISION_ACTION_HOVER:
// attempt to switch to Loiter, if this fails (i.e. flying in manual mode with bad position) do nothing // attempt to switch to Loiter, if this fails (i.e. flying in manual mode with bad position) do nothing
if (failsafe_state_change) { if (failsafe_state_change) {
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_AVOIDANCE)) { if (!copter.set_mode(Mode::Number::LOITER, ModeReason::AVOIDANCE)) {
actual_action = MAV_COLLISION_ACTION_NONE; actual_action = MAV_COLLISION_ACTION_NONE;
} }
} }
@ -106,7 +106,7 @@ void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action)
LogErrorCode::ERROR_RESOLVED); LogErrorCode::ERROR_RESOLVED);
// restore flight mode if requested and user has not changed mode since // restore flight mode if requested and user has not changed mode since
if (copter.control_mode_reason == MODE_REASON_AVOIDANCE) { if (copter.control_mode_reason == ModeReason::AVOIDANCE) {
switch (recovery_action) { switch (recovery_action) {
case AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB: case AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB:
@ -136,10 +136,10 @@ void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action)
void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(Mode::Number mode) void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(Mode::Number mode)
{ {
if (!copter.set_mode(mode, MODE_REASON_AVOIDANCE_RECOVERY)) { if (!copter.set_mode(mode, ModeReason::AVOIDANCE_RECOVERY)) {
// on failure RTL or LAND // on failure RTL or LAND
if (!copter.set_mode(Mode::Number::RTL, MODE_REASON_AVOIDANCE_RECOVERY)) { if (!copter.set_mode(Mode::Number::RTL, ModeReason::AVOIDANCE_RECOVERY)) {
copter.set_mode(Mode::Number::LAND, MODE_REASON_AVOIDANCE_RECOVERY); copter.set_mode(Mode::Number::LAND, ModeReason::AVOIDANCE_RECOVERY);
} }
} }
} }
@ -149,7 +149,7 @@ bool AP_Avoidance_Copter::check_flightmode(bool allow_mode_change)
{ {
// ensure copter is in avoid_adsb mode // ensure copter is in avoid_adsb mode
if (allow_mode_change && copter.control_mode != Mode::Number::AVOID_ADSB) { if (allow_mode_change && copter.control_mode != Mode::Number::AVOID_ADSB) {
if (!copter.set_mode(Mode::Number::AVOID_ADSB, MODE_REASON_AVOIDANCE)) { if (!copter.set_mode(Mode::Number::AVOID_ADSB, ModeReason::AVOIDANCE)) {
// failed to set mode so exit immediately // failed to set mode so exit immediately
return false; return false;
} }

22
ArduCopter/defines.h
View File

@ -30,28 +30,6 @@ enum autopilot_yaw_mode {
#define HIL_MODE_DISABLED 0 #define HIL_MODE_DISABLED 0
#define HIL_MODE_SENSORS 1 #define HIL_MODE_SENSORS 1
enum mode_reason_t {
MODE_REASON_UNKNOWN=0,
MODE_REASON_TX_COMMAND,
MODE_REASON_GCS_COMMAND,
MODE_REASON_RADIO_FAILSAFE,
MODE_REASON_BATTERY_FAILSAFE,
MODE_REASON_GCS_FAILSAFE,
MODE_REASON_EKF_FAILSAFE,
MODE_REASON_GPS_GLITCH,
MODE_REASON_MISSION_END,
MODE_REASON_THROTTLE_LAND_ESCAPE,
MODE_REASON_FENCE_BREACH,
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_BRAKE_TIMEOUT,
MODE_REASON_FLIP_COMPLETE,
MODE_REASON_AVOIDANCE,
MODE_REASON_AVOIDANCE_RECOVERY,
MODE_REASON_THROW_COMPLETE,
MODE_REASON_TERMINATE,
MODE_REASON_TMODE,
};
// Tuning enumeration // Tuning enumeration
enum tuning_func { enum tuning_func {
TUNING_NONE = 0, // TUNING_NONE = 0, //

6
ArduCopter/ekf_check.cpp
View File

@ -158,14 +158,14 @@ void Copter::failsafe_ekf_event()
switch (g.fs_ekf_action) { switch (g.fs_ekf_action) {
case FS_EKF_ACTION_ALTHOLD: case FS_EKF_ACTION_ALTHOLD:
// AltHold // AltHold
if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) { if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)) {
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
} }
break; break;
case FS_EKF_ACTION_LAND: case FS_EKF_ACTION_LAND:
case FS_EKF_ACTION_LAND_EVEN_STABILIZE: case FS_EKF_ACTION_LAND_EVEN_STABILIZE:
default: default:
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
break; break;
} }
} }

32
ArduCopter/events.cpp
View File

@ -22,15 +22,15 @@ void Copter::failsafe_radio_on_event()
// continue landing or other high priority failsafes // continue landing or other high priority failsafes
} else { } else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) { if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) { } else if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL) { } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_RADIO_FAILSAFE); set_mode_SmartRTL_or_RTL(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND) { } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); set_mode_SmartRTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else { // g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND } else { // g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE); set_mode_land_with_pause(ModeReason::RADIO_FAILSAFE);
} }
} }
} }
@ -61,16 +61,16 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
case Failsafe_Action_None: case Failsafe_Action_None:
return; return;
case Failsafe_Action_Land: case Failsafe_Action_Land:
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); set_mode_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break; break;
case Failsafe_Action_RTL: case Failsafe_Action_RTL:
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); set_mode_RTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break; break;
case Failsafe_Action_SmartRTL: case Failsafe_Action_SmartRTL:
set_mode_SmartRTL_or_RTL(MODE_REASON_BATTERY_FAILSAFE); set_mode_SmartRTL_or_RTL(ModeReason::BATTERY_FAILSAFE);
break; break;
case Failsafe_Action_SmartRTL_Land: case Failsafe_Action_SmartRTL_Land:
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); set_mode_SmartRTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break; break;
case Failsafe_Action_Terminate: case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED #if ADVANCED_FAILSAFE == ENABLED
@ -138,11 +138,11 @@ void Copter::failsafe_gcs_check()
g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
// continue mission // continue mission
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) { } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_GCS_FAILSAFE); set_mode_SmartRTL_or_RTL(ModeReason::GCS_FAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) { } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE); set_mode_SmartRTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
} else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL } else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL
set_mode_RTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE); set_mode_RTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
} }
} }
} }
@ -210,7 +210,7 @@ void Copter::failsafe_terrain_on_event()
mode_rtl.restart_without_terrain(); mode_rtl.restart_without_terrain();
#endif #endif
} else { } else {
set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE); set_mode_RTL_or_land_with_pause(ModeReason::TERRAIN_FAILSAFE);
} }
} }
@ -236,7 +236,7 @@ void Copter::gpsglitch_check()
// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot // this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason) void Copter::set_mode_RTL_or_land_with_pause(ModeReason reason)
{ {
// attempt to switch to RTL, if this fails then switch to Land // attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(Mode::Number::RTL, reason)) { if (!set_mode(Mode::Number::RTL, reason)) {
@ -250,7 +250,7 @@ void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason)
// set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts // set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot // this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason) void Copter::set_mode_SmartRTL_or_land_with_pause(ModeReason reason)
{ {
// attempt to switch to SMART_RTL, if this failed then switch to Land // attempt to switch to SMART_RTL, if this failed then switch to Land
if (!set_mode(Mode::Number::SMART_RTL, reason)) { if (!set_mode(Mode::Number::SMART_RTL, reason)) {
@ -263,7 +263,7 @@ void Copter::set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason)
// set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts // set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot // this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_SmartRTL_or_RTL(mode_reason_t reason) void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
{ {
// attempt to switch to SmartRTL, if this failed then attempt to RTL // attempt to switch to SmartRTL, if this failed then attempt to RTL
// if that fails, then land // if that fails, then land

18
ArduCopter/fence.cpp
View File

@ -36,32 +36,32 @@ void Copter::fence_check()
// if more than 100m outside the fence just force a land // if more than 100m outside the fence just force a land
if (fence.get_breach_distance(new_breaches) > AC_FENCE_GIVE_UP_DISTANCE) { if (fence.get_breach_distance(new_breaches) > AC_FENCE_GIVE_UP_DISTANCE) {
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH); set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
} else { } else {
switch (fence_act) { switch (fence_act) {
case AC_FENCE_ACTION_RTL_AND_LAND: case AC_FENCE_ACTION_RTL_AND_LAND:
default: default:
// switch to RTL, if that fails then Land // switch to RTL, if that fails then Land
if (!set_mode(Mode::Number::RTL, MODE_REASON_FENCE_BREACH)) { if (!set_mode(Mode::Number::RTL, ModeReason::FENCE_BREACHED)) {
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH); set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
} }
break; break;
case AC_FENCE_ACTION_ALWAYS_LAND: case AC_FENCE_ACTION_ALWAYS_LAND:
// if always land option mode is specified, land // if always land option mode is specified, land
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH); set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
break; break;
case AC_FENCE_ACTION_SMART_RTL: case AC_FENCE_ACTION_SMART_RTL:
// Try SmartRTL, if that fails, RTL, if that fails Land // Try SmartRTL, if that fails, RTL, if that fails Land
if (!set_mode(Mode::Number::SMART_RTL, MODE_REASON_FENCE_BREACH)) { if (!set_mode(Mode::Number::SMART_RTL, ModeReason::FENCE_BREACHED)) {
if (!set_mode(Mode::Number::RTL, MODE_REASON_FENCE_BREACH)) { if (!set_mode(Mode::Number::RTL, ModeReason::FENCE_BREACHED)) {
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH); set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
} }
} }
break; break;
case AC_FENCE_ACTION_BRAKE: case AC_FENCE_ACTION_BRAKE:
// Try Brake, if that fails Land // Try Brake, if that fails Land
if (!set_mode(Mode::Number::BRAKE, MODE_REASON_FENCE_BREACH)) { if (!set_mode(Mode::Number::BRAKE, ModeReason::FENCE_BREACHED)) {
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH); set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
} }
break; break;
} }

20
ArduCopter/mode.cpp
View File

@ -177,7 +177,7 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
// optional force parameter used to force the flight mode change (used only first time mode is set) // optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully set // returns true if mode was successfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool Copter::set_mode(Mode::Number mode, mode_reason_t reason) bool Copter::set_mode(Mode::Number mode, ModeReason reason)
{ {
// return immediately if we are already in the desired mode // return immediately if we are already in the desired mode
@ -273,6 +273,18 @@ bool Copter::set_mode(Mode::Number mode, mode_reason_t reason)
return true; return true;
} }
bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason)
{
static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
if (reason == ModeReason::GCS_COMMAND && copter.failsafe.radio) {
// don't allow mode changes while in radio failsafe
return false;
}
#endif
return copter.set_mode(static_cast<Mode::Number>(new_mode), ModeReason::GCS_COMMAND);
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode // update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more // called at 100hz or more
void Copter::update_flight_mode() void Copter::update_flight_mode()
@ -542,8 +554,8 @@ void Mode::land_run_horizontal_control()
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
copter.Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); copter.Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high // exit land if throttle is high
if (!set_mode(Mode::Number::LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
set_mode(Mode::Number::ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
} }
} }
@ -738,7 +750,7 @@ void Mode::update_simple_mode(void) {
copter.update_simple_mode(); copter.update_simple_mode();
} }
bool Mode::set_mode(Mode::Number mode, mode_reason_t reason) bool Mode::set_mode(Mode::Number mode, ModeReason reason)
{ {
return copter.set_mode(mode, reason); return copter.set_mode(mode, reason);
} }

4
ArduCopter/mode.h
View File

@ -12,7 +12,7 @@ class Mode {
public: public:
// Auto Pilot Modes enumeration // Auto Pilot Modes enumeration
enum class Number { enum class Number : uint8_t {
STABILIZE = 0, // manual airframe angle with manual throttle STABILIZE = 0, // manual airframe angle with manual throttle
ACRO = 1, // manual body-frame angular rate with manual throttle ACRO = 1, // manual body-frame angular rate with manual throttle
ALT_HOLD = 2, // manual airframe angle with automatic throttle ALT_HOLD = 2, // manual airframe angle with automatic throttle
@ -238,7 +238,7 @@ public:
float get_pilot_desired_climb_rate(float throttle_control); float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle(void); float get_non_takeoff_throttle(void);
void update_simple_mode(void); void update_simple_mode(void);
bool set_mode(Mode::Number mode, mode_reason_t reason); bool set_mode(Mode::Number mode, ModeReason reason);
void set_land_complete(bool b); void set_land_complete(bool b);
GCS_Copter &gcs(); GCS_Copter &gcs();
void Log_Write_Event(Log_Event id); void Log_Write_Event(Log_Event id);

2
ArduCopter/mode_auto.cpp
View File

@ -539,7 +539,7 @@ void ModeAuto::exit_mission()
if (!copter.ap.land_complete) { if (!copter.ap.land_complete) {
// try to enter loiter but if that fails land // try to enter loiter but if that fails land
if (!loiter_start()) { if (!loiter_start()) {
set_mode(Mode::Number::LAND, MODE_REASON_MISSION_END); set_mode(Mode::Number::LAND, ModeReason::MISSION_END);
} }
} else { } else {
// if we've landed it's safe to disarm // if we've landed it's safe to disarm

4
ArduCopter/mode_brake.cpp
View File

@ -63,8 +63,8 @@ void ModeBrake::run()
pos_control->update_z_controller(); pos_control->update_z_controller();
if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) { if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_BRAKE_TIMEOUT)) { if (!copter.set_mode(Mode::Number::LOITER, ModeReason::BRAKE_TIMEOUT)) {
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT); copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT);
} }
} }
} }

8
ArduCopter/mode_flip.cpp
View File

@ -192,9 +192,9 @@ void ModeFlip::run()
// check for successful recovery // check for successful recovery
if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) { if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode // restore original flight mode
if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
// this should never happen but just in case // this should never happen but just in case
copter.set_mode(Mode::Number::STABILIZE, MODE_REASON_UNKNOWN); copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
} }
// log successful completion // log successful completion
Log_Write_Event(DATA_FLIP_END); Log_Write_Event(DATA_FLIP_END);
@ -203,9 +203,9 @@ void ModeFlip::run()
} }
case FlipState::Abandon: case FlipState::Abandon:
// restore original flight mode // restore original flight mode
if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
// this should never happen but just in case // this should never happen but just in case
copter.set_mode(Mode::Number::STABILIZE, MODE_REASON_UNKNOWN); copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
} }
// log abandoning flip // log abandoning flip
AP::logger().Write_Error(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED); AP::logger().Write_Error(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED);

4
ArduCopter/mode_land.cpp
View File

@ -93,7 +93,7 @@ void ModeLand::nogps_run()
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high // exit land if throttle is high
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
} }
if (g.land_repositioning) { if (g.land_repositioning) {
@ -145,7 +145,7 @@ void ModeLand::do_not_use_GPS()
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts // set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot // this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_land_with_pause(mode_reason_t reason) void Copter::set_mode_land_with_pause(ModeReason reason)
{ {
set_mode(Mode::Number::LAND, reason); set_mode(Mode::Number::LAND, reason);
land_pause = true; land_pause = true;

6
ArduCopter/mode_rtl.cpp
View File

@ -119,7 +119,7 @@ void ModeRTL::climb_start()
if (!wp_nav->set_wp_destination(rtl_path.climb_target)) { if (!wp_nav->set_wp_destination(rtl_path.climb_target)) {
// this should not happen because rtl_build_path will have checked terrain data was available // this should not happen because rtl_build_path will have checked terrain data was available
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
copter.set_mode(Mode::Number::LAND, MODE_REASON_TERRAIN_FAILSAFE); copter.set_mode(Mode::Number::LAND, ModeReason::TERRAIN_FAILSAFE);
return; return;
} }
wp_nav->set_fast_waypoint(true); wp_nav->set_fast_waypoint(true);
@ -287,8 +287,8 @@ void ModeRTL::descent_run()
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high // exit land if throttle is high
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { if (!copter.set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
} }
} }

2
ArduCopter/mode_throw.cpp
View File

@ -92,7 +92,7 @@ void ModeThrow::run()
case Mode::Number::LAND: case Mode::Number::LAND:
case Mode::Number::BRAKE: case Mode::Number::BRAKE:
case Mode::Number::LOITER: case Mode::Number::LOITER:
set_mode((Mode::Number)g2.throw_nextmode.get(), MODE_REASON_THROW_COMPLETE); set_mode((Mode::Number)g2.throw_nextmode.get(), ModeReason::THROW_COMPLETE);
break; break;
default: default:
// do nothing // do nothing