Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: add control_mode_reason

This commit is contained in:
Jonathan Challinger 2016-01-25 15:40:41 -08:00 committed by Randy Mackay
parent 6b5ba86f21
commit 1356deab8b
13 changed files with 66 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/Copter.h
View File

@ -247,6 +247,7 @@ 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;
// Structure used to detect changes in the flight mode control switch
struct {
@ -790,7 +791,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();
@ -846,14 +847,14 @@ private:
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(control_mode_t mode);
bool gcs_set_mode(uint8_t mode) { return set_mode((control_mode_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(control_mode_t old_control_mode, control_mode_t new_control_mode);
bool mode_requires_GPS(control_mode_t mode);

8
ArduCopter/GCS_Mavlink.cpp
View File

@ -1220,19 +1220,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_NAV_LOITER_UNLIM:
if (copter.set_mode(LOITER)) {
if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (copter.set_mode(RTL)) {
if (copter.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_NAV_LAND:
if (copter.set_mode(LAND)) {
if (copter.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
@ -1350,7 +1350,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_MISSION_START:
if (copter.motors.armed() && copter.set_mode(AUTO)) {
if (copter.motors.armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
copter.set_auto_armed(true);
if (copter.mission.state() != AP_Mission::MISSION_RUNNING) {
copter.mission.start_or_resume();

2
ArduCopter/commands_logic.cpp
View File

@ -250,7 +250,7 @@ void Copter::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

4
ArduCopter/control_auto.cpp
View File

@ -341,8 +341,8 @@ void Copter::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);
}
}

10
ArduCopter/control_flip.cpp
View File

@ -34,6 +34,7 @@
FlipState flip_state; // current state of flip
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 Copter::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 Copter::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 Copter::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);

10
ArduCopter/control_land.cpp
View File

@ -92,8 +92,8 @@ void Copter::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);
}
}
@ -164,7 +164,7 @@ void Copter::land_nogps_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
set_mode(ALT_HOLD);
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
if (g.land_repositioning) {
@ -260,9 +260,9 @@ void Copter::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 Copter::set_mode_land_with_pause()
void Copter::set_mode_land_with_pause(mode_reason_t reason)
{
set_mode(LAND);
set_mode(LAND, reason);
land_pause = true;
// alert pilot to mode change

8
ArduCopter/control_rtl.cpp
View File

@ -282,8 +282,8 @@ void Copter::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);
}
}
@ -383,8 +383,8 @@ void Copter::rtl_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);
}
}

14
ArduCopter/defines.h
View File

@ -106,6 +106,20 @@ enum control_mode_t {
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, //

6
ArduCopter/ekf_check.cpp
View File

@ -141,12 +141,12 @@ void Copter::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;
}

16
ArduCopter/events.cpp
View File

@ -22,9 +22,9 @@ void Copter::failsafe_radio_on_event()
// continue landing
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause();
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
} else {
set_mode_RTL_or_land_with_pause();
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
}
}
}
@ -57,9 +57,9 @@ void Copter::failsafe_battery_event(void)
init_disarm_motors();
} else {
if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
set_mode_RTL_or_land_with_pause();
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
} else {
set_mode_land_with_pause();
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
}
}
}
@ -118,7 +118,7 @@ void Copter::failsafe_gcs_check()
if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (g.failsafe_gcs != FS_GCS_DISABLED) {
set_mode_RTL_or_land_with_pause();
set_mode_RTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
}
}
}
@ -132,12 +132,12 @@ void Copter::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 Copter::set_mode_RTL_or_land_with_pause()
void Copter::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;

6
ArduCopter/fence.cpp
View File

@ -37,12 +37,12 @@ void Copter::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);
}
}
}

4
ArduCopter/flight_mode.cpp
View File

@ -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 Copter::set_mode(control_mode_t mode)
bool Copter::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,7 @@ bool Copter::set_mode(control_mode_t mode)
// return immediately if we are already in the desired mode
if (mode == control_mode) {
control_mode_reason = reason;
return true;
}
@ -109,6 +110,7 @@ bool Copter::set_mode(control_mode_t mode)
// perform any cleanup required by previous flight mode
exit_mode(control_mode, (control_mode_t)mode);
control_mode = (control_mode_t)mode;
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode);
#if AC_FENCE == ENABLED

16
ArduCopter/switches.cpp
View File

@ -43,7 +43,7 @@ void Copter::read_control_switch()
if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) {
// set flight mode and simple mode setting
if (set_mode((control_mode_t)flight_modes[switch_position].get())) {
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)
@ -251,7 +251,7 @@ void Copter::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;
@ -268,7 +268,7 @@ void Copter::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) {
@ -417,7 +417,7 @@ void Copter::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) {
@ -439,7 +439,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
break;
case AUX_SWITCH_HIGH:
// start an autotuning session
set_mode(AUTOTUNE);
set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND);
break;
}
break;
@ -447,7 +447,7 @@ void Copter::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) {
@ -581,7 +581,7 @@ void Copter::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) {
@ -593,7 +593,7 @@ void Copter::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) {