From b42b1c08c4043d888f82095a404af60506670d42 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 16 Oct 2019 20:49:22 -0700 Subject: [PATCH] Copter: Support new AP_Vehicle::set_mode --- ArduCopter/Copter.h | 15 ++++++++------- ArduCopter/GCS_Mavlink.cpp | 33 +++++++++++---------------------- ArduCopter/GCS_Mavlink.h | 2 -- ArduCopter/RC_Channel.cpp | 6 +++--- ArduCopter/avoidance_adsb.cpp | 14 +++++++------- ArduCopter/defines.h | 22 ---------------------- ArduCopter/ekf_check.cpp | 6 +++--- ArduCopter/events.cpp | 32 ++++++++++++++++---------------- ArduCopter/fence.cpp | 18 +++++++++--------- ArduCopter/mode.cpp | 20 ++++++++++++++++---- ArduCopter/mode.h | 4 ++-- ArduCopter/mode_auto.cpp | 2 +- ArduCopter/mode_brake.cpp | 4 ++-- ArduCopter/mode_flip.cpp | 8 ++++---- ArduCopter/mode_land.cpp | 4 ++-- ArduCopter/mode_rtl.cpp | 6 +++--- ArduCopter/mode_throw.cpp | 2 +- 17 files changed, 88 insertions(+), 110 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 4c36d106cd..94f34af4e8 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -388,10 +388,10 @@ private: // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, 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_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN; + ModeReason prev_control_mode_reason = ModeReason::UNKNOWN; RCMapper rcmap; @@ -719,9 +719,9 @@ private: void failsafe_terrain_set_status(bool data_ok); void failsafe_terrain_on_event(); void gpsglitch_check(); - void set_mode_RTL_or_land_with_pause(mode_reason_t reason); - void set_mode_SmartRTL_or_RTL(mode_reason_t reason); - void set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason); + void set_mode_RTL_or_land_with_pause(ModeReason reason); + void set_mode_SmartRTL_or_RTL(ModeReason reason); + void set_mode_SmartRTL_or_land_with_pause(ModeReason reason); bool should_disarm_on_failsafe(); // failsafe.cpp @@ -782,12 +782,13 @@ private: void log_init(void); // 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 notify_flight_mode(); // 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(); // motor_test.cpp diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 3cb091e112..afc9d7e43d 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -640,19 +640,19 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ } 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_ACCEPTED; 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_ACCEPTED; 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_ACCEPTED; @@ -704,7 +704,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ #if MODE_AUTO_ENABLED == ENABLED case MAV_CMD_MISSION_START: 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); if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) { 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 - if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_GCS_COMMAND)) { - copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND); + if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) { + copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND); } 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); } else if (copter.ap.land_complete) { // 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); } } else { // 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; } @@ -844,13 +844,13 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ if (!shot_mode) { #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); } else { - copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND); + copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND); } #else - copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND); + copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND); #endif } else { // 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; } -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 { if (copter.g2.dev_options.get() & DevOptionVFR_HUDRelativeAlt) { diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index abb4202987..81fb8e1623 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -18,8 +18,6 @@ protected: uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; - bool set_mode(uint8_t mode) override; - bool params_ready() const override; void send_banner() override; diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 72c02ae1e6..37cc054174 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -20,7 +20,7 @@ void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos) 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 if (copter.ap.initialised) { 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) { case HIGH: { // 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 (success) { 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: // flip if switch is on, positive throttle and we're actually flying 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; diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index e5b6555a06..af9bce1d46 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -44,7 +44,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O case MAV_COLLISION_ACTION_RTL: // attempt to switch to RTL, if this fails (i.e. flying in manual mode with bad position) do nothing 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; } } @@ -53,7 +53,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O case MAV_COLLISION_ACTION_HOVER: // attempt to switch to Loiter, if this fails (i.e. flying in manual mode with bad position) do nothing 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; } } @@ -106,7 +106,7 @@ void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action) LogErrorCode::ERROR_RESOLVED); // 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) { 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) { - if (!copter.set_mode(mode, MODE_REASON_AVOIDANCE_RECOVERY)) { + if (!copter.set_mode(mode, ModeReason::AVOIDANCE_RECOVERY)) { // on failure RTL or LAND - if (!copter.set_mode(Mode::Number::RTL, MODE_REASON_AVOIDANCE_RECOVERY)) { - copter.set_mode(Mode::Number::LAND, MODE_REASON_AVOIDANCE_RECOVERY); + if (!copter.set_mode(Mode::Number::RTL, ModeReason::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 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 return false; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 614931a2c5..4eec6ec0cb 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -30,28 +30,6 @@ enum autopilot_yaw_mode { #define HIL_MODE_DISABLED 0 #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 enum tuning_func { TUNING_NONE = 0, // diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index ed486d749d..a69ae93b0a 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -158,14 +158,14 @@ void Copter::failsafe_ekf_event() switch (g.fs_ekf_action) { case FS_EKF_ACTION_ALTHOLD: // AltHold - if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) { - set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); + if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)) { + set_mode_land_with_pause(ModeReason::EKF_FAILSAFE); } break; case FS_EKF_ACTION_LAND: case FS_EKF_ACTION_LAND_EVEN_STABILIZE: default: - set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); + set_mode_land_with_pause(ModeReason::EKF_FAILSAFE); break; } } diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 74c6115382..4c91f239a1 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -22,15 +22,15 @@ void Copter::failsafe_radio_on_event() // continue landing or other high priority failsafes } else { 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) { - 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) { - 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) { - 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 - 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: return; case Failsafe_Action_Land: - set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); + set_mode_land_with_pause(ModeReason::BATTERY_FAILSAFE); break; 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; case Failsafe_Action_SmartRTL: - set_mode_SmartRTL_or_RTL(MODE_REASON_BATTERY_FAILSAFE); + set_mode_SmartRTL_or_RTL(ModeReason::BATTERY_FAILSAFE); break; 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; case Failsafe_Action_Terminate: #if ADVANCED_FAILSAFE == ENABLED @@ -138,11 +138,11 @@ void Copter::failsafe_gcs_check() g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { // continue mission } 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) { - 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 - 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(); #endif } 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 // 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 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 // 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 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 // 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 // if that fails, then land diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 54b2eba8e3..6354161218 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -36,32 +36,32 @@ void Copter::fence_check() // if more than 100m outside the fence just force a land 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 { switch (fence_act) { case AC_FENCE_ACTION_RTL_AND_LAND: default: // switch to RTL, if that fails then Land - if (!set_mode(Mode::Number::RTL, MODE_REASON_FENCE_BREACH)) { - set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH); + if (!set_mode(Mode::Number::RTL, ModeReason::FENCE_BREACHED)) { + set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED); } break; case AC_FENCE_ACTION_ALWAYS_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; case AC_FENCE_ACTION_SMART_RTL: // 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::RTL, MODE_REASON_FENCE_BREACH)) { - set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH); + if (!set_mode(Mode::Number::SMART_RTL, ModeReason::FENCE_BREACHED)) { + if (!set_mode(Mode::Number::RTL, ModeReason::FENCE_BREACHED)) { + set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED); } } break; case AC_FENCE_ACTION_BRAKE: // Try Brake, if that fails Land - if (!set_mode(Mode::Number::BRAKE, MODE_REASON_FENCE_BREACH)) { - set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH); + if (!set_mode(Mode::Number::BRAKE, ModeReason::FENCE_BREACHED)) { + set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED); } break; } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index d162554ccc..937c494db5 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -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) // 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 -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 @@ -273,6 +273,18 @@ bool Copter::set_mode(Mode::Number mode, mode_reason_t reason) 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(new_mode), ModeReason::GCS_COMMAND); +} + // update_flight_mode - calls the appropriate attitude controllers based on flight mode // called at 100hz or more 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){ copter.Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high - if (!set_mode(Mode::Number::LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - set_mode(Mode::Number::ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); + if (!set_mode(Mode::Number::LOITER, ModeReason::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(); } -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); } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 8427ca0b31..bb6b6e769e 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -12,7 +12,7 @@ class Mode { public: // Auto Pilot Modes enumeration - enum class Number { + enum class Number : uint8_t { STABILIZE = 0, // manual airframe angle with manual throttle ACRO = 1, // manual body-frame angular rate with manual 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_non_takeoff_throttle(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); GCS_Copter &gcs(); void Log_Write_Event(Log_Event id); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index fb9200875d..f0152e9090 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -539,7 +539,7 @@ void ModeAuto::exit_mission() if (!copter.ap.land_complete) { // try to enter loiter but if that fails land if (!loiter_start()) { - set_mode(Mode::Number::LAND, MODE_REASON_MISSION_END); + set_mode(Mode::Number::LAND, ModeReason::MISSION_END); } } else { // if we've landed it's safe to disarm diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 7391e4ca58..02f6dd6d4c 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -63,8 +63,8 @@ void ModeBrake::run() pos_control->update_z_controller(); if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) { - if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_BRAKE_TIMEOUT)) { - copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT); + if (!copter.set_mode(Mode::Number::LOITER, ModeReason::BRAKE_TIMEOUT)) { + copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT); } } } diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index f1ee067823..445c027a71 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -192,9 +192,9 @@ void ModeFlip::run() // check for successful recovery if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) { // 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 - copter.set_mode(Mode::Number::STABILIZE, MODE_REASON_UNKNOWN); + copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN); } // log successful completion Log_Write_Event(DATA_FLIP_END); @@ -203,9 +203,9 @@ void ModeFlip::run() } case FlipState::Abandon: // 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 - copter.set_mode(Mode::Number::STABILIZE, MODE_REASON_UNKNOWN); + copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN); } // log abandoning flip AP::logger().Write_Error(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED); diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index c0bffac2d7..ee8301a171 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -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){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // 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) { @@ -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 // 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); land_pause = true; diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 6c332ad5a8..857a3edb55 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -119,7 +119,7 @@ void ModeRTL::climb_start() 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 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; } 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){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high - if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); + if (!copter.set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { + copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); } } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index c225e0df7e..c3740d7f93 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -92,7 +92,7 @@ void ModeThrow::run() case Mode::Number::LAND: case Mode::Number::BRAKE: 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; default: // do nothing