From b187e1f9a66ad192a431d1b1c7492f060f00e72d Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Sat, 13 Aug 2016 00:54:37 -0700 Subject: [PATCH] Plane: add reason to set_mode() --- ArduPlane/ArduPlane.cpp | 4 ++-- ArduPlane/GCS_Mavlink.cpp | 14 +++++++------- ArduPlane/Plane.h | 10 ++++++---- ArduPlane/commands_logic.cpp | 4 ++-- ArduPlane/control_modes.cpp | 2 +- ArduPlane/defines.h | 15 +++++++++++++++ ArduPlane/events.cpp | 30 +++++++++++++++--------------- ArduPlane/geofence.cpp | 8 ++++---- ArduPlane/landing.cpp | 6 ++++-- ArduPlane/system.cpp | 22 ++++++++++++---------- 10 files changed, 68 insertions(+), 47 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index a3da782d2c..ee83d9b4ca 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -510,7 +510,7 @@ void Plane::handle_auto_mode(void) if (mission.state() != AP_Mission::MISSION_RUNNING) { // this should never be reached - set_mode(RTL); + set_mode(RTL, MODE_REASON_MISSION_END); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission"); return; } @@ -779,7 +779,7 @@ void Plane::update_navigation() case RTL: if (quadplane.available() && quadplane.rtl_mode == 1 && nav_controller->reached_loiter_target()) { - set_mode(QRTL); + set_mode(QRTL, MODE_REASON_UNKNOWN); break; } else if (g.rtl_autoland == 1 && !auto_state.checked_for_autoland && diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 301892ffde..bedf3871ff 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1199,7 +1199,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) // location is valid load and set if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == GUIDED)) { - plane.set_mode(GUIDED); + plane.set_mode(GUIDED, MODE_REASON_GCS_COMMAND); plane.guided_WP_loc = requested_position; // add home alt if needed @@ -1267,12 +1267,12 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_NAV_LOITER_UNLIM: - plane.set_mode(LOITER); + plane.set_mode(LOITER, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: - plane.set_mode(RTL); + plane.set_mode(RTL, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; @@ -1334,7 +1334,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_MISSION_START: - plane.set_mode(AUTO); + plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; @@ -1476,19 +1476,19 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: - plane.set_mode(MANUAL); + plane.set_mode(MANUAL, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: - plane.set_mode(AUTO); + plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_STABILIZE_DISARMED: case MAV_MODE_STABILIZE_ARMED: - plane.set_mode(FLY_BY_WIRE_A); + plane.set_mode(FLY_BY_WIRE_A, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 6bd1f97bc6..6aaa929893 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -317,7 +317,9 @@ private: // This is the state of the flight control system // There are multiple states defined such as MANUAL, FBW-A, AUTO enum FlightMode control_mode = INITIALISING; + mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN; enum FlightMode previous_mode = INITIALISING; + mode_reason_t previous_mode_reason = MODE_REASON_UNKNOWN; // Used to maintain the state of the previous control switch position // This is set to 254 when we need to re-read the switch @@ -915,9 +917,9 @@ private: void autotune_restore(void); void autotune_enable(bool enable); bool fly_inverted(void); - void failsafe_short_on_event(enum failsafe_state fstype); - void failsafe_long_on_event(enum failsafe_state fstype); - void failsafe_short_off_event(); + 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 low_battery_event(void); void update_events(void); uint8_t max_fencepoints(void); @@ -989,7 +991,7 @@ private: void init_ardupilot(); void startup_ground(void); enum FlightMode get_previous_mode(); - void set_mode(enum FlightMode mode); + void set_mode(enum FlightMode mode, mode_reason_t reason); bool mavlink_set_mode(uint8_t mode); void exit_mode(enum FlightMode mode); void check_long_failsafe(); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index f6edacd78a..fb04ddee06 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -81,7 +81,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: - set_mode(RTL); + set_mode(RTL, MODE_REASON_UNKNOWN); break; case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: @@ -989,7 +989,7 @@ bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd) void Plane::exit_mission_callback() { if (control_mode == AUTO) { - set_mode(RTL); + set_mode(RTL, MODE_REASON_MISSION_END); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL"); } } diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 54c9749760..ddef328860 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -41,7 +41,7 @@ void Plane::read_control_switch() return; } - set_mode((enum FlightMode)(flight_modes[switchPosition].get())); + set_mode((enum FlightMode)(flight_modes[switchPosition].get()), MODE_REASON_TX_COMMAND); oldSwitchPosition = switchPosition; } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index aa1eb48379..f04500ecc8 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -68,6 +68,21 @@ enum FlightMode { QRTL = 21 }; +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, +}; + // type of stick mixing enabled enum StickMixing { STICK_MIXING_DISABLED = 0, diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 10a99f47f1..4ce0262b5d 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -2,7 +2,7 @@ #include "Plane.h" -void Plane::failsafe_short_on_event(enum failsafe_state fstype) +void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason) { // This is how to handle a short loss of control signal failsafe. failsafe.state = fstype; @@ -21,9 +21,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) failsafe.saved_mode = control_mode; failsafe.saved_mode_set = 1; if(g.short_fs_action == 2) { - set_mode(FLY_BY_WIRE_A); + set_mode(FLY_BY_WIRE_A, reason); } else { - set_mode(CIRCLE); + set_mode(CIRCLE, reason); } break; @@ -32,7 +32,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) case QHOVER: failsafe.saved_mode = control_mode; failsafe.saved_mode_set = 1; - set_mode(QLAND); + set_mode(QLAND, reason); break; case AUTO: @@ -43,9 +43,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) failsafe.saved_mode = control_mode; failsafe.saved_mode_set = 1; if(g.short_fs_action == 2) { - set_mode(FLY_BY_WIRE_A); + set_mode(FLY_BY_WIRE_A, reason); } else { - set_mode(CIRCLE); + set_mode(CIRCLE, reason); } } break; @@ -60,7 +60,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); } -void Plane::failsafe_long_on_event(enum failsafe_state fstype) +void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason) { // This is how to handle a long loss of control signal failsafe. gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on, "); @@ -83,16 +83,16 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) parachute_release(); #endif } else if (g.long_fs_action == 2) { - set_mode(FLY_BY_WIRE_A); + set_mode(FLY_BY_WIRE_A, reason); } else { - set_mode(RTL); + set_mode(RTL, reason); } break; case QSTABILIZE: case QHOVER: case QLOITER: - set_mode(QLAND); + set_mode(QLAND, reason); break; case AUTO: @@ -104,9 +104,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) parachute_release(); #endif } else if (g.long_fs_action == 2) { - set_mode(FLY_BY_WIRE_A); + set_mode(FLY_BY_WIRE_A, reason); } else if (g.long_fs_action == 1) { - set_mode(RTL); + set_mode(RTL, reason); } break; @@ -122,7 +122,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); } -void Plane::failsafe_short_off_event() +void Plane::failsafe_short_off_event(mode_reason_t reason) { // We're back in radio contact gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off"); @@ -132,7 +132,7 @@ void Plane::failsafe_short_off_event() // -------------------------------------------------------- if (control_mode == CIRCLE && failsafe.saved_mode_set) { failsafe.saved_mode_set = 0; - set_mode(failsafe.saved_mode); + set_mode(failsafe.saved_mode, reason); } } @@ -146,7 +146,7 @@ void Plane::low_battery_event(void) if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL && flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE && flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - set_mode(RTL); + set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE); aparm.throttle_cruise.load(); } failsafe.low_battery = true; diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index b9d9f9c5ed..fce453f343 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -292,7 +292,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(get_previous_mode()); + set_mode(get_previous_mode(), MODE_REASON_GCS_COMMAND); } return; } @@ -311,7 +311,7 @@ void Plane::geofence_check(bool altitude_check_only) struct Location loc; // Never trigger a fence breach in the final stage of landing - if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { + if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) { return; } @@ -380,9 +380,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(RTL); + set_mode(RTL, MODE_REASON_FENCE_BREACH); } else { - set_mode(GUIDED); + set_mode(GUIDED, MODE_REASON_FENCE_BREACH); } g.auto_trim.set(saved_auto_trim); diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index 5a5000ee77..f03bd7e81c 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -33,7 +33,7 @@ bool Plane::verify_land() mission.resume(); if (!success) { // on a restart failure lets RTL or else the plane may fly away with nowhere to go! - set_mode(RTL); + set_mode(RTL, MODE_REASON_MISSION_END); } // make sure to return false so it leaves the mission index alone } @@ -368,7 +368,9 @@ bool Plane::jump_to_landing_sequence(void) uint16_t land_idx = mission.get_landing_sequence_start(); if (land_idx != 0) { if (mission.set_current_cmd(land_idx)) { - set_mode(AUTO); + + // in case we're in RTL + set_mode(AUTO, MODE_REASON_UNKNOWN); //if the mission has ended it has to be restarted if (mission.state() == AP_Mission::MISSION_STOPPED) { diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 3c17a89ac9..9e08dad4e3 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -245,7 +245,7 @@ void Plane::init_ardupilot() // choose the nav controller set_nav_controller(); - set_mode((FlightMode)g.initial_mode.get()); + set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN); // set the correct flight mode // --------------------------- @@ -265,7 +265,7 @@ void Plane::init_ardupilot() //******************************************************************************** void Plane::startup_ground(void) { - set_mode(INITIALISING); + set_mode(INITIALISING, MODE_REASON_UNKNOWN); gcs_send_text(MAV_SEVERITY_INFO," Ground start"); @@ -311,7 +311,7 @@ enum FlightMode Plane::get_previous_mode() { return previous_mode; } -void Plane::set_mode(enum FlightMode mode) +void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) { if(control_mode == mode) { // don't switch modes if we are already in the correct mode. @@ -352,6 +352,8 @@ void Plane::set_mode(enum FlightMode mode) // set mode previous_mode = control_mode; control_mode = mode; + previous_mode_reason = control_mode_reason; + control_mode_reason = reason; if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) { // restore last gains @@ -518,7 +520,7 @@ bool Plane::mavlink_set_mode(uint8_t mode) case QLOITER: case QLAND: case QRTL: - set_mode((enum FlightMode)mode); + set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND); return true; } return false; @@ -552,19 +554,19 @@ void Plane::check_long_failsafe() flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { if (failsafe.state == FAILSAFE_SHORT && (tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) { - failsafe_long_on_event(FAILSAFE_LONG); + failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO && failsafe.last_heartbeat_ms != 0 && (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { - failsafe_long_on_event(FAILSAFE_GCS); + failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT && failsafe.last_heartbeat_ms != 0 && (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { - failsafe_long_on_event(FAILSAFE_GCS); + failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && gcs[0].last_radio_status_remrssi_ms != 0 && (tnow - gcs[0].last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) { - failsafe_long_on_event(FAILSAFE_GCS); + failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); } } else { // We do not change state but allow for user to change mode @@ -588,13 +590,13 @@ void Plane::check_short_failsafe() flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { // The condition is checked and the flag ch3_failsafe is set in radio.cpp if(failsafe.ch3_failsafe) { - failsafe_short_on_event(FAILSAFE_SHORT); + failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE); } } if(failsafe.state == FAILSAFE_SHORT) { if(!failsafe.ch3_failsafe) { - failsafe_short_off_event(); + failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE); } } }