From 3f94f0d51779177974f541a6e810f151ceb07e66 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 16 Oct 2019 20:50:07 -0700 Subject: [PATCH] Sub: Support new AP_Vehicle::set_mode --- ArduSub/GCS_Mavlink.cpp | 11 +++-------- ArduSub/GCS_Mavlink.h | 1 - ArduSub/Sub.h | 7 ++++--- ArduSub/commands_logic.cpp | 2 +- ArduSub/control_surface.cpp | 2 +- ArduSub/defines.h | 20 +------------------- ArduSub/failsafe.cpp | 16 ++++++++-------- ArduSub/flight_mode.cpp | 8 +++++++- ArduSub/joystick.cpp | 18 +++++++++--------- 9 files changed, 34 insertions(+), 51 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 699088bf3b..f828b13732 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -458,13 +458,13 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon { switch (packet.command) { case MAV_CMD_NAV_LOITER_UNLIM: - if (!sub.set_mode(POSHOLD, MODE_REASON_GCS_COMMAND)) { + if (!sub.set_mode(POSHOLD, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; case MAV_CMD_NAV_LAND: - if (!sub.set_mode(SURFACE, MODE_REASON_GCS_COMMAND)) { + if (!sub.set_mode(SURFACE, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; @@ -494,7 +494,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon return MAV_RESULT_FAILED; case MAV_CMD_MISSION_START: - if (sub.motors.armed() && sub.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) { + if (sub.motors.armed() && sub.set_mode(AUTO, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; @@ -825,11 +825,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long return MAV_RESULT_FAILED; } -bool GCS_MAVLINK_Sub::set_mode(uint8_t mode) -{ - return sub.set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND); -} - int32_t GCS_MAVLINK_Sub::global_position_int_alt() const { if (!sub.ap.depth_sensor_present) { return 0; diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index c6e416c575..3921ee59f7 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -18,7 +18,6 @@ protected: uint8_t sysid_my_gcs() const override; - bool set_mode(uint8_t mode) override; bool should_zero_rc_outputs_on_reboot() const override { return true; } MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index e857be85b9..19802dc439 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -230,10 +230,10 @@ private: // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, control_mode_t control_mode; - mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN; + ModeReason control_mode_reason = ModeReason::UNKNOWN; control_mode_t prev_control_mode; - mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN; + ModeReason prev_control_mode_reason = ModeReason::UNKNOWN; #if RCMAP_ENABLED == ENABLED RCMapper rcmap; @@ -549,7 +549,8 @@ private: void mainloop_failsafe_enable(); void mainloop_failsafe_disable(); void fence_check(); - bool set_mode(control_mode_t mode, mode_reason_t reason); + bool set_mode(control_mode_t mode, ModeReason reason); + bool set_mode(const uint8_t mode, const ModeReason reason) override; void update_flight_mode(); void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode); bool mode_requires_GPS(control_mode_t mode); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 97c5529f6a..577e09ca05 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -216,7 +216,7 @@ void Sub::exit_mission() // Try to enter loiter, if that fails, go to depth hold if (!auto_loiter_start()) { - set_mode(ALT_HOLD, MODE_REASON_MISSION_END); + set_mode(ALT_HOLD, ModeReason::MISSION_END); } } diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index 3fc6170988..3b05f40529 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -35,7 +35,7 @@ void Sub::surface_run() // Already at surface, hold depth at surface if (ap.at_surface) { - set_mode(ALT_HOLD, MODE_REASON_SURFACE_COMPLETE); + set_mode(ALT_HOLD, ModeReason::SURFACE_COMPLETE); } // convert pilot input to lean angles diff --git a/ArduSub/defines.h b/ArduSub/defines.h index e9b8f6dfa9..1b8777c568 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -30,7 +30,7 @@ enum autopilot_yaw_mode { }; // Auto Pilot Modes enumeration -enum control_mode_t { +enum control_mode_t : uint8_t { STABILIZE = 0, // manual angle with manual depth/throttle ACRO = 1, // manual body-frame angular rate with manual depth/throttle ALT_HOLD = 2, // manual angle with automatic depth/throttle @@ -43,24 +43,6 @@ enum control_mode_t { MOTOR_DETECT = 20 // Automatically detect motors orientation }; -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_SURFACE_ESCAPE, - MODE_REASON_FENCE_BREACH, - MODE_REASON_TERRAIN_FAILSAFE, - MODE_REASON_SURFACE_COMPLETE, - MODE_REASON_LEAK_FAILSAFE, - MODE_REASON_BAD_DEPTH -}; - // Acro Trainer types #define ACRO_TRAINER_DISABLED 0 #define ACRO_TRAINER_LEVELING 1 diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index 23339793fc..856330f850 100644 --- a/ArduSub/failsafe.cpp +++ b/ArduSub/failsafe.cpp @@ -90,7 +90,7 @@ void Sub::failsafe_sensors_check() if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) { // This should always succeed - if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) { + if (!set_mode(MANUAL, ModeReason::BAD_DEPTH)) { // We should never get here arming.disarm(); } @@ -157,7 +157,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action) switch((Failsafe_Action)action) { case Failsafe_Action_Surface: - set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE); + set_mode(SURFACE, ModeReason::BATTERY_FAILSAFE); break; case Failsafe_Action_Disarm: arming.disarm(); @@ -300,7 +300,7 @@ void Sub::failsafe_leak_check() // Handle failsafe action if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) { - set_mode(SURFACE, MODE_REASON_LEAK_FAILSAFE); + set_mode(SURFACE, ModeReason::LEAK_FAILSAFE); } } @@ -347,11 +347,11 @@ void Sub::failsafe_gcs_check() if (g.failsafe_gcs == FS_GCS_DISARM) { arming.disarm(); } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) { - if (!set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE)) { + if (!set_mode(ALT_HOLD, ModeReason::GCS_FAILSAFE)) { arming.disarm(); } } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) { - if (!set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE)) { + if (!set_mode(SURFACE, ModeReason::GCS_FAILSAFE)) { arming.disarm(); } } @@ -477,14 +477,14 @@ void Sub::failsafe_terrain_act() { switch (g.failsafe_terrain) { case FS_TERRAIN_HOLD: - if (!set_mode(POSHOLD, MODE_REASON_TERRAIN_FAILSAFE)) { - set_mode(ALT_HOLD, MODE_REASON_TERRAIN_FAILSAFE); + if (!set_mode(POSHOLD, ModeReason::TERRAIN_FAILSAFE)) { + set_mode(ALT_HOLD, ModeReason::TERRAIN_FAILSAFE); } AP_Notify::events.failsafe_mode_change = 1; break; case FS_TERRAIN_SURFACE: - set_mode(SURFACE, MODE_REASON_TERRAIN_FAILSAFE); + set_mode(SURFACE, ModeReason::TERRAIN_FAILSAFE); AP_Notify::events.failsafe_mode_change = 1; break; diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index ac0322566e..1e1077a998 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -2,7 +2,7 @@ // change flight mode and perform any necessary initialisation // returns true if mode was successfully set -bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) +bool Sub::set_mode(control_mode_t mode, ModeReason reason) { // boolean to record if flight mode could be set bool success = false; @@ -99,6 +99,12 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) return success; } +bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason) +{ + static_assert(sizeof(control_mode_t) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); + return sub.set_mode((control_mode_t)new_mode, reason); +} + // update_flight_mode - calls the appropriate attitude controllers based on flight mode // called at 100hz or more void Sub::update_flight_mode() diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index e4e2e90b2f..e00119fd31 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -35,7 +35,7 @@ void Sub::init_joystick() lights1 = RC_Channels::rc_channel(8)->get_radio_min(); lights2 = RC_Channels::rc_channel(9)->get_radio_min(); - set_mode(MANUAL, MODE_REASON_TX_COMMAND); // Initialize flight mode + set_mode(MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode if (g.numGainSettings < 1) { g.numGainSettings.set_and_save(1); @@ -158,28 +158,28 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) break; case JSButton::button_function_t::k_mode_manual: - set_mode(MANUAL, MODE_REASON_TX_COMMAND); + set_mode(MANUAL, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_stabilize: - set_mode(STABILIZE, MODE_REASON_TX_COMMAND); + set_mode(STABILIZE, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_depth_hold: - set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND); + set_mode(ALT_HOLD, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_auto: - set_mode(AUTO, MODE_REASON_TX_COMMAND); + set_mode(AUTO, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_guided: - set_mode(GUIDED, MODE_REASON_TX_COMMAND); + set_mode(GUIDED, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_circle: - set_mode(CIRCLE, MODE_REASON_TX_COMMAND); + set_mode(CIRCLE, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_acro: - set_mode(ACRO, MODE_REASON_TX_COMMAND); + set_mode(ACRO, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_poshold: - set_mode(POSHOLD, MODE_REASON_TX_COMMAND); + set_mode(POSHOLD, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mount_center: