diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 61a0a8f3fb..b8e3d4ad40 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -796,7 +796,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) switch (packet.command) { case MAV_CMD_NAV_RETURN_TO_LAUNCH: - rover.set_mode(rover.mode_rtl); + rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; @@ -861,7 +861,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_MISSION_START: - rover.set_mode(rover.mode_auto); + rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; @@ -918,19 +918,19 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) switch (static_cast(packet.param1)) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: - rover.set_mode(rover.mode_manual); + rover.set_mode(rover.mode_manual, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: - rover.set_mode(rover.mode_auto); + rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_STABILIZE_DISARMED: case MAV_MODE_STABILIZE_ARMED: - rover.set_mode(rover.mode_learning); + rover.set_mode(rover.mode_learning, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 3d26dda4be..e6e4c65276 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -526,7 +526,7 @@ void Rover::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by DataFlash Log_Write_Startup(TYPE_GROUNDSTART_MSG); - DataFlash.Log_Write_Mode(control_mode->mode_number()); + DataFlash.Log_Write_Mode(control_mode->mode_number(), control_mode_reason); Log_Write_Home_And_Origin(); gps.Write_DataFlash_Log_Startup_messages(); } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index c640affc55..a63ca27e58 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -233,6 +233,7 @@ private: // This is the state of the flight control system // There are multiple states defined such as MANUAL, AUTO, ... Mode *control_mode; + mode_reason_t control_mode_reason = MODE_REASON_INITIALISED; // Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch @@ -467,7 +468,7 @@ private: void gcs_retry_deferred(void); Mode *control_mode_from_num(enum mode num); - bool set_mode(Mode &mode); + bool set_mode(Mode &mode, mode_reason_t reason); bool mavlink_set_mode(uint8_t mode); void do_erase_logs(void); diff --git a/APMrover2/afs_rover.cpp b/APMrover2/afs_rover.cpp index e8f3763079..3ad7d7951c 100644 --- a/APMrover2/afs_rover.cpp +++ b/APMrover2/afs_rover.cpp @@ -21,7 +21,7 @@ void AP_AdvancedFailsafe_Rover::terminate_vehicle(void) rover.disarm_motors(); // Set to HOLD mode - rover.set_mode(rover.mode_hold); + rover.set_mode(rover.mode_hold, MODE_REASON_CRASH_FAILSAFE); } /* diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 870a907a45..459cdba5e8 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -126,7 +126,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) void Rover::exit_mission() { gcs().send_text(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD"); - set_mode(mode_hold); + set_mode(mode_hold, MODE_REASON_MISSION_END); } // verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run @@ -205,7 +205,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd) void Rover::do_RTL(void) { - set_mode(mode_rtl); + set_mode(mode_rtl, MODE_REASON_MISSION_COMMAND); } void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd) diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index e18f1884bc..c442b821d4 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -72,7 +72,7 @@ void Rover::read_control_switch() Mode *new_mode = control_mode_from_num((enum mode)modes[switchPosition].get()); if (new_mode != nullptr) { - set_mode(*new_mode); + set_mode(*new_mode, MODE_REASON_TX_COMMAND); } oldSwitchPosition = switchPosition; @@ -155,7 +155,7 @@ void Rover::read_trim_switch() } } else if (control_mode == &mode_auto) { // if SW7 is ON in AUTO = set to RTL - set_mode(mode_rtl); + set_mode(mode_rtl, MODE_REASON_TX_COMMAND); break; } } diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index bea241f496..69a94f6d59 100644 --- a/APMrover2/crash_check.cpp +++ b/APMrover2/crash_check.cpp @@ -38,7 +38,7 @@ void Rover::crash_check() // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); // change mode to hold and disarm - set_mode(mode_hold); + set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE); if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { disarm_motors(); } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 217068bea7..1800a12b11 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -95,7 +95,9 @@ enum mode { #define MAVLINK_SET_ATT_TYPE_MASK_ATTITUDE_IGNORE (1<<7) // Error message sub systems and error codes -#define ERROR_SUBSYSTEM_CRASH_CHECK 12 +#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9 +#define ERROR_SUBSYSTEM_FLIGHT_MODE 10 +#define ERROR_SUBSYSTEM_CRASH_CHECK 12 // subsystem specific error codes -- crash checker #define ERROR_CODE_CRASH_CHECK_CRASH 1 @@ -106,3 +108,13 @@ enum fs_crash_action { }; #define DISTANCE_HOME_MAX 0.5f // Distance max to home location before changing it when disarm + +enum mode_reason_t { + MODE_REASON_INITIALISED = 0, + MODE_REASON_TX_COMMAND, + MODE_REASON_GCS_COMMAND, + MODE_REASON_FAILSAFE, + MODE_REASON_MISSION_END, + MODE_REASON_CRASH_FAILSAFE, + MODE_REASON_MISSION_COMMAND +}; diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index ae9adb83a8..3bdaabc520 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -82,10 +82,10 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) case 0: break; case 1: - set_mode(mode_rtl); + set_mode(mode_rtl, MODE_REASON_FAILSAFE); break; case 2: - set_mode(mode_hold); + set_mode(mode_hold, MODE_REASON_FAILSAFE); break; } } diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index de77c08559..1ccbdaab91 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -23,6 +23,6 @@ void ModeRTL::update_navigation() { // no loitering around the wp with the rover, goes direct to the wp position if (rover.verify_RTL()) { - rover.set_mode(rover.mode_hold); + rover.set_mode(rover.mode_hold, MODE_REASON_MISSION_END); } } diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 510edbd1f4..622ede421f 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -213,7 +213,7 @@ void Rover::init_ardupilot() if (initial_mode == nullptr) { initial_mode = &mode_initializing; } - set_mode(*initial_mode); + set_mode(*initial_mode, MODE_REASON_INITIALISED); // set the correct flight mode @@ -232,7 +232,7 @@ void Rover::init_ardupilot() //********************************************************************************* void Rover::startup_ground(void) { - set_mode(mode_initializing); + set_mode(mode_initializing, MODE_REASON_INITIALISED); gcs().send_text(MAV_SEVERITY_INFO, " Ground start"); @@ -283,7 +283,7 @@ void Rover::set_reverse(bool reverse) in_reverse = reverse; } -bool Rover::set_mode(Mode &new_mode) +bool Rover::set_mode(Mode &new_mode, mode_reason_t reason) { if (control_mode == &new_mode) { // don't switch modes if we are already in the correct mode. @@ -292,6 +292,9 @@ bool Rover::set_mode(Mode &new_mode) Mode &old_mode = *control_mode; if (!new_mode.enter()) { + // Log error that we failed to enter desired flight mode + Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE, new_mode.mode_number()); + gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed"); return false; } @@ -304,7 +307,8 @@ bool Rover::set_mode(Mode &new_mode) old_mode.exit(); if (should_log(MASK_LOG_MODE)) { - DataFlash.Log_Write_Mode(control_mode->mode_number()); + control_mode_reason = reason; + DataFlash.Log_Write_Mode(control_mode->mode_number(), reason); } notify_mode((enum mode)control_mode->mode_number()); @@ -320,7 +324,7 @@ bool Rover::mavlink_set_mode(uint8_t mode) if (new_mode == nullptr) { return false; } - return set_mode(*new_mode); + return set_mode(*new_mode, MODE_REASON_GCS_COMMAND); } void Rover::startup_INS_ground(void)