From c1f05b9e25169d16c7691600b538711babc8a156 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Sun, 17 Apr 2016 21:38:21 -0700 Subject: [PATCH] Sub: Match copter mode change stuff --- ArduSub/APM_Config.h | 1 + ArduSub/GCS_Mavlink.cpp | 23 ++++-- ArduSub/Log.cpp | 2 +- ArduSub/Sub.h | 24 ++++-- ArduSub/commands_logic.cpp | 2 +- ArduSub/config.h | 5 -- ArduSub/control_auto.cpp | 4 +- ArduSub/control_flip.cpp | 12 +-- ArduSub/control_land.cpp | 10 +-- ArduSub/control_rtl.cpp | 4 +- ArduSub/defines.h | 21 ++++- ArduSub/ekf_check.cpp | 6 +- ArduSub/events.cpp | 164 +++++++++++-------------------------- ArduSub/fence.cpp | 6 +- ArduSub/flight_mode.cpp | 25 ++++-- ArduSub/motors.cpp | 2 +- ArduSub/setup.cpp | 2 +- ArduSub/switches.cpp | 14 ++-- ArduSub/takeoff.cpp | 2 + 19 files changed, 153 insertions(+), 176 deletions(-) diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h index 8a656fc50b..244bae5920 100644 --- a/ArduSub/APM_Config.h +++ b/ArduSub/APM_Config.h @@ -33,6 +33,7 @@ // features below are disabled by default on all boards //#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity) //#define GNDEFFECT_COMPENSATION ENABLED // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground) +//#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE // disable mode changes from GCS during Radio failsafes. Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 2200c5c2eb..c4f2139954 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -59,6 +59,8 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) // APM does in any mode, as that is defined as "system finds its own goal // positions", which APM does not currently do break; + default: + break; } // all modes except INITIALISING have some form of manual @@ -173,6 +175,8 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) case SPORT: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; break; + default: + break; } // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED) @@ -1001,7 +1005,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_SET_MODE: // MAV ID: 11 { - handle_set_mode(msg, FUNCTOR_BIND(&sub, &Sub::set_mode, bool, uint8_t)); +#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE + if (!sub.failsafe.radio) { + handle_set_mode(msg, FUNCTOR_BIND(&sub, &Sub::gcs_set_mode, bool, uint8_t)); + } else { + // don't allow mode changes while in radio failsafe + mavlink_msg_command_ack_send_buf(msg, chan, MAVLINK_MSG_ID_SET_MODE, MAV_RESULT_FAILED); + } +#else + handle_set_mode(msg, FUNCTOR_BIND(&sub, &Sub::gcs_set_mode, bool, uint8_t)); +#endif break; } @@ -1214,19 +1227,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_NAV_LOITER_UNLIM: - if (sub.set_mode(LOITER)) { + if (sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: - if (sub.set_mode(RTL)) { + if (sub.set_mode(RTL, MODE_REASON_GCS_COMMAND)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_NAV_LAND: - if (sub.set_mode(LAND)) { + if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) { result = MAV_RESULT_ACCEPTED; } break; @@ -1344,7 +1357,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_MISSION_START: - if (sub.motors.armed() && sub.set_mode(AUTO)) { + if (sub.motors.armed() && sub.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) { sub.set_auto_armed(true); if (sub.mission.state() != AP_Mission::MISSION_RUNNING) { sub.mission.start_or_resume(); diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 02f7166a5b..378648f7ec 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -731,7 +731,7 @@ void Sub::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by DataFlash DataFlash.Log_Write_Message("Frame: " FRAME_CONFIG_STRING); - DataFlash.Log_Write_Mode(control_mode); + DataFlash.Log_Write_Mode(control_mode, control_mode_reason); } diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 85d4ecc010..3762bb4c16 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -249,7 +249,11 @@ private: // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, - int8_t control_mode; + control_mode_t control_mode; + mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN; + + control_mode_t prev_control_mode; + mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN; // Structure used to detect changes in the flight mode control switch struct { @@ -761,7 +765,7 @@ private: void land_nogps_run(); float get_land_descent_speed(); void land_do_not_use_GPS(); - void set_mode_land_with_pause(); + void set_mode_land_with_pause(mode_reason_t reason); bool landing_with_GPS(); bool loiter_init(bool ignore_checks); void loiter_run(); @@ -812,24 +816,26 @@ private: void esc_calibration_startup_check(); void esc_calibration_passthrough(); void esc_calibration_auto(); + bool should_disarm_on_failsafe(); void failsafe_radio_on_event(); void failsafe_radio_off_event(); void failsafe_battery_event(void); void failsafe_gcs_check(); void failsafe_gcs_off_event(void); - void set_mode_RTL_or_land_with_pause(); + void set_mode_RTL_or_land_with_pause(mode_reason_t reason); void update_events(); void failsafe_enable(); void failsafe_disable(); void fence_check(); void fence_send_mavlink_status(mavlink_channel_t chan); - bool set_mode(uint8_t mode); + bool set_mode(control_mode_t mode, mode_reason_t reason); + bool gcs_set_mode(uint8_t mode) { return set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND); } void update_flight_mode(); - void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode); - bool mode_requires_GPS(uint8_t mode); - bool mode_has_manual_throttle(uint8_t mode); - bool mode_allows_arming(uint8_t mode, bool arming_from_gcs); - void notify_flight_mode(uint8_t mode); + void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode); + bool mode_requires_GPS(control_mode_t mode); + bool mode_has_manual_throttle(control_mode_t mode); + bool mode_allows_arming(control_mode_t mode, bool arming_from_gcs); + void notify_flight_mode(control_mode_t mode); void check_dynamic_flight(void); void read_inertia(); void read_inertial_altitude(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index dbcb4c9e0d..03b0b054df 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -250,7 +250,7 @@ void Sub::exit_mission() if(!ap.land_complete) { // try to enter loiter but if that fails land if(!auto_loiter_start()) { - set_mode(LAND); + set_mode(LAND, MODE_REASON_MISSION_END); } }else{ #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED diff --git a/ArduSub/config.h b/ArduSub/config.h index 422978dbcc..0b885d0f3e 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -161,11 +161,6 @@ # define GNDEFFECT_COMPENSATION DISABLED #endif -// possible values for FS_GCS parameter -#define FS_GCS_DISABLED 0 -#define FS_GCS_ENABLED_ALWAYS_RTL 1 -#define FS_GCS_ENABLED_CONTINUE_MISSION 2 - // Radio failsafe while using RC_override #ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS # define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS 1000 // RC Radio failsafe triggers after 1 second while using RC_override from ground station diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index a5a0be1241..6d60ab9d85 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -324,8 +324,8 @@ void Sub::auto_land_run() if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && 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 (!set_mode(LOITER)) { - set_mode(ALT_HOLD); + if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { + set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } diff --git a/ArduSub/control_flip.cpp b/ArduSub/control_flip.cpp index 2dd2f64326..fa40cc35f9 100644 --- a/ArduSub/control_flip.cpp +++ b/ArduSub/control_flip.cpp @@ -33,7 +33,8 @@ #define FLIP_PITCH_FORWARD -1 // used to set flip_dir FlipState flip_state; // current state of flip -uint8_t flip_orig_control_mode; // flight mode when flip was initated +control_mode_t flip_orig_control_mode; // flight mode when flip was initated +mode_reason_t flip_orig_control_mode_reason; uint32_t flip_start_time; // time since flip began int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll right) int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back) @@ -63,6 +64,7 @@ bool Sub::flip_init(bool ignore_checks) // capture original flight mode so that we can return to it after completion flip_orig_control_mode = control_mode; + flip_orig_control_mode_reason = control_mode_reason; // initialise state flip_state = Flip_Start; @@ -199,9 +201,9 @@ void Sub::flip_run() // check for successful recovery if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) { // restore original flight mode - if (!set_mode(flip_orig_control_mode)) { + if (!set_mode(flip_orig_control_mode, flip_orig_control_mode_reason)) { // this should never happen but just in case - set_mode(STABILIZE); + set_mode(STABILIZE, MODE_REASON_UNKNOWN); } // log successful completion Log_Write_Event(DATA_FLIP_END); @@ -210,9 +212,9 @@ void Sub::flip_run() case Flip_Abandon: // restore original flight mode - if (!set_mode(flip_orig_control_mode)) { + if (!set_mode(flip_orig_control_mode, flip_orig_control_mode_reason)) { // this should never happen but just in case - set_mode(STABILIZE); + set_mode(STABILIZE, MODE_REASON_UNKNOWN); } // log abandoning flip Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED); diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp index c136d801a6..825a928d01 100644 --- a/ArduSub/control_land.cpp +++ b/ArduSub/control_land.cpp @@ -87,8 +87,8 @@ void Sub::land_gps_run() if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && 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 (!set_mode(LOITER)) { - set_mode(ALT_HOLD); + if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { + set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } @@ -182,7 +182,7 @@ void Sub::land_nogps_run() #else // disarm when the landing detector says we've landed if (ap.at_surface) { - set_mode(ALT_HOLD); + set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } #endif return; @@ -243,9 +243,9 @@ void Sub::land_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 Sub::set_mode_land_with_pause() +void Sub::set_mode_land_with_pause(mode_reason_t reason) { - set_mode(LAND); + set_mode(LAND, reason); land_pause = true; // alert pilot to mode change diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp index 42a67a516c..dc0a1146eb 100644 --- a/ArduSub/control_rtl.cpp +++ b/ArduSub/control_rtl.cpp @@ -293,8 +293,8 @@ void Sub::rtl_descent_run() if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && 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 (!set_mode(LOITER)) { - set_mode(ALT_HOLD); + if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { + set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } diff --git a/ArduSub/defines.h b/ArduSub/defines.h index ff22d92f49..f263bb29b6 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -94,7 +94,7 @@ enum aux_sw_func { #define HIL_MODE_SENSORS 1 // Auto Pilot Modes enumeration -enum autopilot_modes { +enum control_mode_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 @@ -114,6 +114,20 @@ enum autopilot_modes { THROW = 18 // throw to launch mode using inertial/GPS system, no pilot input }; +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 +}; + // Tuning enumeration enum tuning_func { TUNING_NONE = 0, // @@ -428,6 +442,11 @@ enum ThrowModeState { #define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe #define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe +// GCS failsafe definitions (FS_GCS_ENABLE parameter) +#define FS_GCS_DISABLED 0 +#define FS_GCS_ENABLED_ALWAYS_RTL 1 +#define FS_GCS_ENABLED_CONTINUE_MISSION 2 + // EKF failsafe definitions (FS_EKF_ACTION parameter) #define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe #define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe diff --git a/ArduSub/ekf_check.cpp b/ArduSub/ekf_check.cpp index 19f5af3d7f..6f0469ca19 100644 --- a/ArduSub/ekf_check.cpp +++ b/ArduSub/ekf_check.cpp @@ -141,12 +141,12 @@ void Sub::failsafe_ekf_event() switch (g.fs_ekf_action) { case FS_EKF_ACTION_ALTHOLD: // AltHold - if (failsafe.radio || !set_mode(ALT_HOLD)) { - set_mode_land_with_pause(); + if (failsafe.radio || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) { + set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); } break; default: - set_mode_land_with_pause(); + set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); break; } diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp index 0abbeb4cee..ce97c8f799 100644 --- a/ArduSub/events.cpp +++ b/ArduSub/events.cpp @@ -13,76 +13,20 @@ void Sub::failsafe_radio_on_event() return; } - // This is how to handle a failsafe. - switch(control_mode) { - case STABILIZE: - case ACRO: - // if throttle is zero OR vehicle is landed disarm motors - if (ap.throttle_zero || ap.land_complete) { - init_disarm_motors(); - - // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately - }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { - set_mode_land_with_pause(); - - // if far from home then RTL - } else if(home_distance > FS_CLOSE_TO_HOME_CM) { - // switch to RTL or if that fails, LAND - set_mode_RTL_or_land_with_pause(); - - // We have no GPS or are very close to home so we will land - }else{ - set_mode_land_with_pause(); + if (should_disarm_on_failsafe()) { + init_disarm_motors(); + } else { + if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) { + // continue mission + } else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) { + // continue landing + } else { + if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { + set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE); + } else { + set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); } - break; - - case AUTO: - // if mission has not started AND vehicle is landed, disarm motors - if (!ap.auto_armed && ap.land_complete) { - init_disarm_motors(); - - // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately - } else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { - set_mode_land_with_pause(); - - // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL - } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) { - if (home_distance > FS_CLOSE_TO_HOME_CM) { - // switch to RTL or if that fails, LAND - set_mode_RTL_or_land_with_pause(); - }else{ - // We are very close to home so we will land - set_mode_land_with_pause(); - } - } - // failsafe_throttle must be FS_THR_ENABLED_CONTINUE_MISSION so no need to do anything - break; - - case LAND: - // continue to land if battery failsafe is also active otherwise fall through to default handling - if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) { - break; - } - // no break - default: - // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold - // if landed disarm - if (ap.land_complete) { - init_disarm_motors(); - - // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately - } else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { - set_mode_land_with_pause(); - - // if far from home then RTL - } else if(home_distance > FS_CLOSE_TO_HOME_CM) { - // switch to RTL or if that fails, LAND - set_mode_RTL_or_land_with_pause(); - }else{ - // We have no GPS or are very close to home so we will land - set_mode_land_with_pause(); - } - break; + } } // log the error to the dataflash @@ -108,52 +52,17 @@ void Sub::failsafe_battery_event(void) } // failsafe check - if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { - switch(control_mode) { - case STABILIZE: - case ACRO: - // if throttle is zero OR vehicle is landed disarm motors - if (ap.throttle_zero || ap.land_complete) { - init_disarm_motors(); - }else{ - // set mode to RTL or LAND - if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) { - // switch to RTL or if that fails, LAND - set_mode_RTL_or_land_with_pause(); - }else{ - set_mode_land_with_pause(); - } - } - break; - case AUTO: - // if mission has not started AND vehicle is landed, disarm motors - if (!ap.auto_armed && ap.land_complete) { - init_disarm_motors(); - - // set mode to RTL or LAND - } else if (home_distance > FS_CLOSE_TO_HOME_CM) { - // switch to RTL or if that fails, LAND - set_mode_RTL_or_land_with_pause(); - } else { - set_mode_land_with_pause(); - } - break; - default: - // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold - // if landed disarm - if (ap.land_complete) { - init_disarm_motors(); - - // set mode to RTL or LAND - } else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) { - // switch to RTL or if that fails, LAND - set_mode_RTL_or_land_with_pause(); - } else { - set_mode_land_with_pause(); - } - break; - } - } + if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { + if (should_disarm_on_failsafe()) { + init_disarm_motors(); + } else { + if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) { + set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); + } else { + set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); + } + } + } // set the low battery flag set_failsafe_battery(true); @@ -216,18 +125,37 @@ void Sub::failsafe_gcs_off_event(void) // 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 Sub::set_mode_RTL_or_land_with_pause() +void Sub::set_mode_RTL_or_land_with_pause(mode_reason_t reason) { // attempt to switch to RTL, if this fails then switch to Land - if (!set_mode(RTL)) { + if (!set_mode(RTL, reason)) { // set mode to land will trigger mode change notification to pilot - set_mode_land_with_pause(); + set_mode_land_with_pause(reason); } else { // alert pilot to mode change AP_Notify::events.failsafe_mode_change = 1; } } +bool Sub::should_disarm_on_failsafe() { + switch(control_mode) { + case STABILIZE: + case ACRO: + // if throttle is zero OR vehicle is landed disarm motors + return ap.throttle_zero || ap.land_complete; + break; + case AUTO: + // if mission has not started AND vehicle is landed, disarm motors + return !ap.auto_armed && ap.land_complete; + break; + default: + // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold + // if landed disarm + return ap.land_complete; + break; + } +} + void Sub::update_events() { ServoRelayEvents.update_events(); diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index 8578c15083..a076f61eb1 100644 --- a/ArduSub/fence.cpp +++ b/ArduSub/fence.cpp @@ -37,12 +37,12 @@ void Sub::fence_check() }else{ // if we are within 100m of the fence, RTL if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { - if (!set_mode(RTL)) { - set_mode(LAND); + if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) { + set_mode(LAND, MODE_REASON_FENCE_BREACH); } }else{ // if more than 100m outside the fence just force a land - set_mode(LAND); + set_mode(LAND, MODE_REASON_FENCE_BREACH); } } } diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index 499d12d205..a34852cdba 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -11,7 +11,7 @@ // optional force parameter used to force the flight mode change (used only first time mode is set) // returns true if mode was succesfully 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 Sub::set_mode(uint8_t mode) +bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) { // boolean to record if flight mode could be set bool success = false; @@ -19,6 +19,10 @@ bool Sub::set_mode(uint8_t mode) // return immediately if we are already in the desired mode if (mode == control_mode) { + prev_control_mode = control_mode; + prev_control_mode_reason = control_mode_reason; + + control_mode_reason = reason; return true; } @@ -100,8 +104,13 @@ bool Sub::set_mode(uint8_t mode) if (success) { // perform any cleanup required by previous flight mode exit_mode(control_mode, mode); + + prev_control_mode = control_mode; + prev_control_mode_reason = control_mode_reason; + control_mode = mode; - DataFlash.Log_Write_Mode(control_mode); + control_mode_reason = reason; + DataFlash.Log_Write_Mode(control_mode, control_mode_reason); #if AC_FENCE == ENABLED // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover @@ -198,11 +207,13 @@ void Sub::update_flight_mode() case THROW: throw_run(); break; + default: + break; } } // exit_mode - high level call to organise cleanup as a flight mode is exited -void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) +void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode) { #if AUTOTUNE_ENABLED == ENABLED if (old_control_mode == AUTOTUNE) { @@ -235,7 +246,7 @@ void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) } // returns true or false whether mode requires GPS -bool Sub::mode_requires_GPS(uint8_t mode) { +bool Sub::mode_requires_GPS(control_mode_t mode) { switch(mode) { case AUTO: case GUIDED: @@ -255,7 +266,7 @@ bool Sub::mode_requires_GPS(uint8_t mode) { } // mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle) -bool Sub::mode_has_manual_throttle(uint8_t mode) { +bool Sub::mode_has_manual_throttle(control_mode_t mode) { switch(mode) { case ACRO: case STABILIZE: @@ -269,7 +280,7 @@ bool Sub::mode_has_manual_throttle(uint8_t mode) { // mode_allows_arming - returns true if vehicle can be armed in the specified mode // arming_from_gcs should be set to true if the arming request comes from the ground station -bool Sub::mode_allows_arming(uint8_t mode, bool arming_from_gcs) { +bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) { if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && mode == GUIDED)) { return true; } @@ -277,7 +288,7 @@ bool Sub::mode_allows_arming(uint8_t mode, bool arming_from_gcs) { } // notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device -void Sub::notify_flight_mode(uint8_t mode) { +void Sub::notify_flight_mode(control_mode_t mode) { switch(mode) { case AUTO: case GUIDED: diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 81e4cec942..2c55f0b8af 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -207,7 +207,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs) Log_Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed - DataFlash.Log_Write_Mode(control_mode); + DataFlash.Log_Write_Mode(control_mode, control_mode_reason); // reenable failsafe failsafe_enable(); diff --git a/ArduSub/setup.cpp b/ArduSub/setup.cpp index fb5e9b9528..222d54f24e 100644 --- a/ArduSub/setup.cpp +++ b/ArduSub/setup.cpp @@ -359,7 +359,7 @@ void Sub::report_flight_modes() print_divider(); for(int16_t i = 0; i < 6; i++ ) { - print_switch(i, flight_modes[i], BIT_IS_SET(g.simple_modes, i)); + print_switch(i, (control_mode_t)flight_modes[i].get(), BIT_IS_SET(g.simple_modes, i)); } print_blanks(2); } diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index d98a866ae5..ff19e768eb 100644 --- a/ArduSub/switches.cpp +++ b/ArduSub/switches.cpp @@ -43,7 +43,7 @@ void Sub::read_control_switch() if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) { // set flight mode and simple mode setting - if (set_mode(flight_modes[switch_position])) { + if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)) { // play a tone if (control_switch_state.debounced_switch_position != -1) { // alert user to mode change failure (except if autopilot is just starting up) @@ -258,7 +258,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_FLIP: // flip if switch is on, positive throttle and we're actually flying if(ch_flag == AUX_SWITCH_HIGH) { - set_mode(FLIP); + set_mode(FLIP, MODE_REASON_TX_COMMAND); } break; @@ -275,7 +275,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_RTL: if (ch_flag == AUX_SWITCH_HIGH) { // engage RTL (if not possible we remain in current flight mode) - set_mode(RTL); + set_mode(RTL, MODE_REASON_TX_COMMAND); }else{ // return to flight mode switch's flight mode if we are currently in RTL if (control_mode == RTL) { @@ -424,7 +424,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_AUTO: if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(AUTO); + set_mode(AUTO, MODE_REASON_TX_COMMAND); }else{ // return to flight mode switch's flight mode if we are currently in AUTO if (control_mode == AUTO) { @@ -454,7 +454,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_LAND: if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(LAND); + set_mode(LAND, MODE_REASON_TX_COMMAND); }else{ // return to flight mode switch's flight mode if we are currently in LAND if (control_mode == LAND) { @@ -576,7 +576,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_BRAKE: // brake flight mode if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(BRAKE); + set_mode(BRAKE, MODE_REASON_TX_COMMAND); }else{ // return to flight mode switch's flight mode if we are currently in BRAKE if (control_mode == BRAKE) { @@ -588,7 +588,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_THROW: // throw flight mode if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(THROW); + set_mode(THROW, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in throw mode if (control_mode == THROW) { diff --git a/ArduSub/takeoff.cpp b/ArduSub/takeoff.cpp index c4ac7fc6b2..fd7d416e01 100644 --- a/ArduSub/takeoff.cpp +++ b/ArduSub/takeoff.cpp @@ -41,6 +41,8 @@ bool Sub::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) set_auto_armed(true); takeoff_timer_start(takeoff_alt_cm); return true; + default: + return false; } } return false;