From ee96ec7f0d13671ee8f89bc5996347c94761840b Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 16 Oct 2019 20:49:32 -0700 Subject: [PATCH] Plane: Support new AP_Vehicle::set_mode --- ArduPlane/ArduPlane.cpp | 6 +++--- ArduPlane/GCS_Mavlink.cpp | 22 +++++--------------- ArduPlane/GCS_Mavlink.h | 2 -- ArduPlane/Plane.h | 17 ++++++++-------- ArduPlane/RC_Channel.cpp | 2 +- ArduPlane/afs_plane.cpp | 2 +- ArduPlane/avoidance_adsb.cpp | 16 +++++++-------- ArduPlane/commands_logic.cpp | 4 ++-- ArduPlane/control_modes.cpp | 2 +- ArduPlane/defines.h | 22 -------------------- ArduPlane/events.cpp | 22 ++++++++++---------- ArduPlane/geofence.cpp | 6 +++--- ArduPlane/mode.h | 2 +- ArduPlane/mode_auto.cpp | 2 +- ArduPlane/quadplane.cpp | 6 +++--- ArduPlane/soaring.cpp | 10 ++++----- ArduPlane/system.cpp | 39 +++++++++++++++++++++++------------- 17 files changed, 79 insertions(+), 103 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index b6f5a5463d..4a0cadd102 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -453,7 +453,7 @@ void Plane::update_navigation() are within the maximum of the stopping distance and the RTL_RADIUS */ - set_mode(mode_qrtl, MODE_REASON_UNKNOWN); + set_mode(mode_qrtl, ModeReason::UNKNOWN); break; } else if (g.rtl_autoland == 1 && !auto_state.checked_for_autoland && @@ -462,7 +462,7 @@ void Plane::update_navigation() // we've reached the RTL point, see if we have a landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO - set_mode(mode_auto, MODE_REASON_UNKNOWN); + set_mode(mode_auto, ModeReason::UNKNOWN); } // prevent running the expensive jump_to_landing_sequence @@ -474,7 +474,7 @@ void Plane::update_navigation() // Go directly to the landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO - set_mode(mode_auto, MODE_REASON_UNKNOWN); + set_mode(mode_auto, ModeReason::UNKNOWN); } // prevent running the expensive jump_to_landing_sequence diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 5d90ffce08..9809935c1b 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -774,7 +774,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com // location is valid load and set if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { - plane.set_mode(plane.mode_guided, MODE_REASON_GCS_COMMAND); + plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); plane.guided_WP_loc = requested_position; // add home alt if needed @@ -827,11 +827,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } case MAV_CMD_NAV_LOITER_UNLIM: - plane.set_mode(plane.mode_loiter, MODE_REASON_GCS_COMMAND); + plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND); return MAV_RESULT_ACCEPTED; case MAV_CMD_NAV_RETURN_TO_LAUNCH: - plane.set_mode(plane.mode_rtl, MODE_REASON_GCS_COMMAND); + plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND); return MAV_RESULT_ACCEPTED; case MAV_CMD_NAV_TAKEOFF: { @@ -845,13 +845,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } case MAV_CMD_MISSION_START: - plane.set_mode(plane.mode_auto, MODE_REASON_GCS_COMMAND); + plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); return MAV_RESULT_ACCEPTED; case MAV_CMD_DO_LAND_START: // attempt to switch to next DO_LAND_START command in the mission if (plane.mission.jump_to_landing_sequence()) { - plane.set_mode(plane.mode_auto, MODE_REASON_UNKNOWN); + plane.set_mode(plane.mode_auto, ModeReason::UNKNOWN); return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; @@ -1386,18 +1386,6 @@ void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, const ma } } -/* - set_mode() wrapper for MAVLink SET_MODE - */ -bool GCS_MAVLINK_Plane::set_mode(const uint8_t mode) -{ - Mode *new_mode = plane.mode_from_mode_num((enum Mode::Number)mode); - if (new_mode == nullptr) { - return false; - } - return plane.set_mode(*new_mode, MODE_REASON_GCS_COMMAND); -} - uint64_t GCS_MAVLINK_Plane::capabilities() const { return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT | diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index b96d585eb4..a240016e1d 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -19,8 +19,6 @@ protected: uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; - bool set_mode(uint8_t mode) override; - MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index faa10e1774..7bd9d7e31f 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -294,9 +294,9 @@ private: // This is the state of the flight control system // There are multiple states defined such as MANUAL, FBW-A, AUTO Mode *control_mode = &mode_initializing; - mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN; + ModeReason control_mode_reason = ModeReason::UNKNOWN; Mode *previous_mode = &mode_initializing; - mode_reason_t previous_mode_reason = MODE_REASON_UNKNOWN; + ModeReason previous_mode_reason = ModeReason::UNKNOWN; // time of last mode change uint32_t last_mode_change_ms; @@ -855,10 +855,10 @@ private: void autotune_restore(void); void autotune_enable(bool enable); bool fly_inverted(void); - void failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason); - void failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason); - void failsafe_short_off_event(mode_reason_t reason); - void failsafe_long_off_event(mode_reason_t reason); + void failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason); + void failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason); + void failsafe_short_off_event(ModeReason reason); + void failsafe_long_off_event(ModeReason reason); void handle_battery_failsafe(const char* type_str, const int8_t action); uint8_t max_fencepoints(void) const; Vector2l get_fence_point_with_index(uint8_t i) const; @@ -906,8 +906,9 @@ private: void rpm_update(void); void init_ardupilot(); void startup_ground(void); - bool set_mode(Mode& new_mode, const mode_reason_t reason); - bool set_mode_by_number(const Mode::Number new_mode_number, const mode_reason_t reason); + bool set_mode(Mode& new_mode, const ModeReason reason); + bool set_mode(const uint8_t mode, const ModeReason reason) override; + bool set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason); Mode *mode_from_mode_num(const enum Mode::Number num); void check_long_failsafe(); void check_short_failsafe(); diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 9f83cca7b0..98f7a32ec3 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -33,7 +33,7 @@ void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number, switch(ch_flag) { case HIGH: { // engage mode (if not possible we remain in current flight mode) - const bool success = plane.set_mode_by_number(number, MODE_REASON_TX_COMMAND); + const bool success = plane.set_mode_by_number(number, ModeReason::RC_COMMAND); if (plane.control_mode != &plane.mode_initializing) { if (success) { AP_Notify::events.user_mode_change = 1; diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index cf2a6f55a4..b4c72291da 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -18,7 +18,7 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) { if (plane.quadplane.available() && _terminate_action == TERMINATE_ACTION_LAND) { // perform a VTOL landing - plane.set_mode(plane.mode_qland, MODE_REASON_FENCE_BREACH); + plane.set_mode(plane.mode_qland, ModeReason::FENCE_BREACHED); return; } diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index 7e163614da..a3045acdd7 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -36,16 +36,16 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob case MAV_COLLISION_ACTION_RTL: if (failsafe_state_change) { - plane.set_mode(plane.mode_rtl, MODE_REASON_AVOIDANCE); + plane.set_mode(plane.mode_rtl, ModeReason::AVOIDANCE); } break; case MAV_COLLISION_ACTION_HOVER: if (failsafe_state_change) { if (plane.quadplane.is_flying()) { - plane.set_mode(plane.mode_qloiter, MODE_REASON_AVOIDANCE); + plane.set_mode(plane.mode_qloiter, ModeReason::AVOIDANCE); } else { - plane.set_mode(plane.mode_loiter, MODE_REASON_AVOIDANCE); + plane.set_mode(plane.mode_loiter, ModeReason::AVOIDANCE); } } break; @@ -105,7 +105,7 @@ void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action) gcs().send_text(MAV_SEVERITY_INFO, "Avoid: Resuming with action: %d", recovery_action); // restore flight mode if requested and user has not changed mode since - if (plane.control_mode_reason == MODE_REASON_AVOIDANCE) { + if (plane.control_mode_reason == ModeReason::AVOIDANCE) { switch (recovery_action) { case AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB: @@ -113,16 +113,16 @@ void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action) break; case AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE: - plane.set_mode_by_number(prev_control_mode_number, MODE_REASON_AVOIDANCE_RECOVERY); + plane.set_mode_by_number(prev_control_mode_number, ModeReason::AVOIDANCE_RECOVERY); break; case AP_AVOIDANCE_RECOVERY_RTL: - plane.set_mode(plane.mode_rtl, MODE_REASON_AVOIDANCE_RECOVERY); + plane.set_mode(plane.mode_rtl, ModeReason::AVOIDANCE_RECOVERY); break; case AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER: if (prev_control_mode_number == Mode::Number::AUTO) { - plane.set_mode(plane.mode_auto, MODE_REASON_AVOIDANCE_RECOVERY); + plane.set_mode(plane.mode_auto, ModeReason::AVOIDANCE_RECOVERY); } // else do nothing, same as AP_AVOIDANCE_RECOVERY_LOITER break; @@ -139,7 +139,7 @@ bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change) { // ensure plane is in avoid_adsb mode if (allow_mode_change && plane.control_mode != &plane.mode_avoidADSB) { - plane.set_mode(plane.mode_avoidADSB, MODE_REASON_AVOIDANCE); + plane.set_mode(plane.mode_avoidADSB, ModeReason::AVOIDANCE); } // check flight mode diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index d60d0d4551..679398ead3 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -75,7 +75,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: - set_mode(mode_rtl, MODE_REASON_UNKNOWN); + set_mode(mode_rtl, ModeReason::UNKNOWN); break; case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: @@ -962,7 +962,7 @@ bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd) void Plane::exit_mission_callback() { if (control_mode == &mode_auto) { - set_mode(mode_rtl, MODE_REASON_MISSION_END); + set_mode(mode_rtl, ModeReason::MISSION_END); gcs().send_text(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL"); } } diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 43153fe86c..33a0677b67 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -116,7 +116,7 @@ void Plane::read_control_switch() return; } - set_mode_by_number((enum Mode::Number)flight_modes[switchPosition].get(), MODE_REASON_TX_COMMAND); + set_mode_by_number((enum Mode::Number)flight_modes[switchPosition].get(), ModeReason::RC_COMMAND); oldSwitchPosition = switchPosition; } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 6a40e845e5..b8db26b542 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -45,28 +45,6 @@ enum failsafe_action_long { FS_ACTION_LONG_PARACHUTE = 3, }; -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_FENCE_BREACH, - MODE_REASON_AVOIDANCE, - MODE_REASON_AVOIDANCE_RECOVERY, - MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING, - MODE_REASON_SOARING_THERMAL_DETECTED, - MODE_REASON_SOARING_IN_THERMAL, - MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED, - MODE_REASON_VTOL_FAILED_TRANSITION, - MODE_REASON_UNAVAILABLE, - MODE_REASON_VTOL_FAILED_TAKEOFF, -}; - // type of stick mixing enabled enum StickMixing { STICK_MIXING_DISABLED = 0, diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 39db0e2c77..4ed2df9ce3 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -1,11 +1,11 @@ #include "Plane.h" -void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason) +void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason) { // This is how to handle a short loss of control signal failsafe. failsafe.state = fstype; failsafe.short_timer_ms = millis(); - gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, reason); + gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, static_cast(reason)); switch (control_mode->mode_number()) { case Mode::Number::MANUAL: @@ -65,10 +65,10 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number()); } -void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason) +void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason) { // This is how to handle a long loss of control signal failsafe. - gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on: type=%u/reason=%u", fstype, reason); + gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on: type=%u/reason=%u", fstype, static_cast(reason)); // If the GCS is locked up we allow control to revert to RC RC_Channels::clear_overrides(); failsafe.state = fstype; @@ -131,10 +131,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t rea gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number()); } -void Plane::failsafe_short_off_event(mode_reason_t reason) +void Plane::failsafe_short_off_event(ModeReason reason) { // We're back in radio contact - gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", reason); + gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", static_cast(reason)); failsafe.state = FAILSAFE_NONE; // re-read the switch so we can return to our preferred mode @@ -145,10 +145,10 @@ void Plane::failsafe_short_off_event(mode_reason_t reason) } } -void Plane::failsafe_long_off_event(mode_reason_t reason) +void Plane::failsafe_long_off_event(ModeReason reason) { // We're back in radio contact - gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event off: reason=%u", reason); + gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event off: reason=%u", static_cast(reason)); failsafe.state = FAILSAFE_NONE; } @@ -157,7 +157,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) switch ((Failsafe_Action)action) { case Failsafe_Action_QLand: if (quadplane.available()) { - plane.set_mode(mode_qland, MODE_REASON_BATTERY_FAILSAFE); + plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE); break; } FALLTHROUGH; @@ -165,7 +165,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland) { // never stop a landing if we were already committed if (plane.mission.jump_to_landing_sequence()) { - plane.set_mode(mode_auto, MODE_REASON_BATTERY_FAILSAFE); + plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE); break; } } @@ -173,7 +173,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) case Failsafe_Action_RTL: if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland ) { // never stop a landing if we were already committed - set_mode(mode_rtl, MODE_REASON_BATTERY_FAILSAFE); + set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE); aparm.throttle_cruise.load(); } break; diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index 9f1f5a319e..bdd7904a7c 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -336,7 +336,7 @@ void Plane::geofence_check(bool altitude_check_only) guided_WP_loc.lat == geofence_state->guided_lat && guided_WP_loc.lng == geofence_state->guided_lng) { geofence_state->old_switch_position = 254; - set_mode(*previous_mode, MODE_REASON_GCS_COMMAND); + set_mode(*previous_mode, ModeReason::GCS_COMMAND); } return; } @@ -424,9 +424,9 @@ void Plane::geofence_check(bool altitude_check_only) int8_t saved_auto_trim = g.auto_trim; g.auto_trim.set(0); if (g.fence_action == FENCE_ACTION_RTL) { - set_mode(mode_rtl, MODE_REASON_FENCE_BREACH); + set_mode(mode_rtl, ModeReason::FENCE_BREACHED); } else { - set_mode(mode_guided, MODE_REASON_FENCE_BREACH); + set_mode(mode_guided, ModeReason::FENCE_BREACHED); } g.auto_trim.set(saved_auto_trim); diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 64e4ce0c74..169000efe1 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -14,7 +14,7 @@ public: // Auto Pilot modes // ---------------- - enum Number { + enum Number : uint8_t { MANUAL = 0, CIRCLE = 1, STABILIZE = 2, diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index b3d29cfd65..0cb6b26b9a 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -49,7 +49,7 @@ void ModeAuto::update() if (plane.mission.state() != AP_Mission::MISSION_RUNNING) { // this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if: // restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point - plane.set_mode(plane.mode_rtl, MODE_REASON_MISSION_END); + plane.set_mode(plane.mode_rtl, ModeReason::MISSION_END); gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission"); return; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1dd1bd4784..9bc41204df 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1456,7 +1456,7 @@ void QuadPlane::update_transition(void) (transition_failure > 0) && ((now - transition_start_ms) > ((uint32_t)transition_failure * 1000))) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit"); - plane.set_mode(plane.mode_qland, MODE_REASON_VTOL_FAILED_TRANSITION); + plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TRANSITION); } float aspeed; @@ -2576,13 +2576,13 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) // check for failure conditions if (is_positive(takeoff_failure_scalar) && ((now - takeoff_start_time_ms) > takeoff_time_limit_ms)) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff within time limit"); - plane.set_mode(plane.mode_qland, MODE_REASON_VTOL_FAILED_TAKEOFF); + plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF); return false; } if (is_positive(maximum_takeoff_airspeed) && (plane.airspeed.get_airspeed() > maximum_takeoff_airspeed)) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff, excessive wind"); - plane.set_mode(plane.mode_qland, MODE_REASON_VTOL_FAILED_TAKEOFF); + plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF); return false; } diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index b025d98334..bdf18311b0 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -24,7 +24,7 @@ void Plane::update_soaring() { case Mode::Number::CRUISE: if (!g2.soaring_controller.suppress_throttle()) { gcs().send_text(MAV_SEVERITY_INFO, "Soaring: forcing RTL"); - set_mode(mode_rtl, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING); + set_mode(mode_rtl, ModeReason::SOARING_FBW_B_WITH_MOTOR_RUNNING); } break; case Mode::Number::LOITER: @@ -51,7 +51,7 @@ void Plane::update_soaring() { if (g2.soaring_controller.check_thermal_criteria()) { gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter"); - set_mode(mode_loiter, MODE_REASON_SOARING_THERMAL_DETECTED); + set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED); } break; @@ -64,14 +64,14 @@ void Plane::update_soaring() { switch (previous_mode->mode_number()) { case Mode::Number::FLY_BY_WIRE_B: gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL"); - set_mode(mode_rtl, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); + set_mode(mode_rtl, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED); break; case Mode::Number::CRUISE: { // return to cruise with old ground course CruiseState cruise = cruise_state; gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE"); - set_mode(mode_cruise, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); + set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED); cruise_state = cruise; set_target_altitude_current(); break; @@ -79,7 +79,7 @@ void Plane::update_soaring() { case Mode::Number::AUTO: gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO"); - set_mode(mode_auto, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); + set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED); break; default: diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index ca8eab5686..e04cdd1bef 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -163,7 +163,7 @@ void Plane::init_ardupilot() // choose the nav controller set_nav_controller(); - set_mode_by_number((enum Mode::Number)g.initial_mode.get(), MODE_REASON_UNKNOWN); + set_mode_by_number((enum Mode::Number)g.initial_mode.get(), ModeReason::UNKNOWN); // set the correct flight mode // --------------------------- @@ -194,7 +194,7 @@ void Plane::init_ardupilot() //******************************************************************************** void Plane::startup_ground(void) { - set_mode(mode_initializing, MODE_REASON_UNKNOWN); + set_mode(mode_initializing, ModeReason::UNKNOWN); #if (GROUND_START_DELAY > 0) gcs().send_text(MAV_SEVERITY_NOTICE,"Ground start with delay"); @@ -241,7 +241,7 @@ void Plane::startup_ground(void) } -bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason) +bool Plane::set_mode(Mode &new_mode, const ModeReason reason) { if (control_mode == &new_mode) { // don't switch modes if we are already in the correct mode. @@ -251,7 +251,7 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason) #if !QAUTOTUNE_ENABLED if (&new_mode == &plane.mode_qautotune) { gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled"); - set_mode(plane.mode_qhover, MODE_REASON_UNAVAILABLE); + set_mode(plane.mode_qhover, ModeReason::UNAVAILABLE); return false; } #endif @@ -259,7 +259,7 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason) // backup current control_mode and previous_mode Mode &old_previous_mode = *previous_mode; Mode &old_mode = *control_mode; - const mode_reason_t previous_mode_reason_backup = previous_mode_reason; + const ModeReason previous_mode_reason_backup = previous_mode_reason; // update control_mode assuming success // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode @@ -309,7 +309,18 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason) return true; } -bool Plane::set_mode_by_number(const Mode::Number new_mode_number, const mode_reason_t reason) +bool Plane::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"); + Mode *mode = plane.mode_from_mode_num(static_cast(new_mode)); + if (mode == nullptr) { + gcs().send_text(MAV_SEVERITY_INFO, "Error: invalid mode number: %u", (unsigned)new_mode); + return false; + } + return set_mode(*mode, reason); +} + +bool Plane::set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason) { Mode *new_mode = plane.mode_from_mode_num(new_mode_number); if (new_mode == nullptr) { @@ -332,20 +343,20 @@ void Plane::check_long_failsafe() } if (failsafe.rc_failsafe && (tnow - radio_timeout_ms) > g.fs_timeout_long*1000) { - failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE); + failsafe_long_on_event(FAILSAFE_LONG, ModeReason::RADIO_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == &mode_auto && failsafe.last_heartbeat_ms != 0 && (tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) { - failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); + failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT && failsafe.last_heartbeat_ms != 0 && (tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) { - failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); + failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && gcs().chan(0) != nullptr && gcs().chan(0)->last_radio_status_remrssi_ms != 0 && (tnow - gcs().chan(0)->last_radio_status_remrssi_ms) > g.fs_timeout_long*1000) { - failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); + failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE); } } else { uint32_t timeout_seconds = g.fs_timeout_long; @@ -356,10 +367,10 @@ void Plane::check_long_failsafe() // We do not change state but allow for user to change mode if (failsafe.state == FAILSAFE_GCS && (tnow - failsafe.last_heartbeat_ms) < timeout_seconds*1000) { - failsafe_long_off_event(MODE_REASON_GCS_FAILSAFE); + failsafe_long_off_event(ModeReason::GCS_FAILSAFE); } else if (failsafe.state == FAILSAFE_LONG && !failsafe.rc_failsafe) { - failsafe_long_off_event(MODE_REASON_RADIO_FAILSAFE); + failsafe_long_off_event(ModeReason::RADIO_FAILSAFE); } } } @@ -373,13 +384,13 @@ void Plane::check_short_failsafe() flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) { // The condition is checked and the flag rc_failsafe is set in radio.cpp if(failsafe.rc_failsafe) { - failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE); + failsafe_short_on_event(FAILSAFE_SHORT, ModeReason::RADIO_FAILSAFE); } } if(failsafe.state == FAILSAFE_SHORT) { if(!failsafe.rc_failsafe || g.fs_action_short == FS_ACTION_SHORT_DISABLED) { - failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE); + failsafe_short_off_event(ModeReason::RADIO_FAILSAFE); } } }