Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Match copter mode change stuff

Browse Source
This commit is contained in:
Rustom Jehangir 2016-04-17 21:38:21 -07:00 committed by Andrew Tridgell
parent 99ad33411b
commit c1f05b9e25
19 changed files with 153 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/APM_Config.h
View File

@ -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)

23
ArduSub/GCS_Mavlink.cpp
View File

@ -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();

2
ArduSub/Log.cpp
View File

@ -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);
}

24
ArduSub/Sub.h
View File

@ -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();

2
ArduSub/commands_logic.cpp
View File

@ -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

5
ArduSub/config.h
View File

@ -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

4
ArduSub/control_auto.cpp
View File

@ -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);
}
}

12
ArduSub/control_flip.cpp
View File

@ -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);

10
ArduSub/control_land.cpp
View File

@ -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

4
ArduSub/control_rtl.cpp
View File

@ -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);
}
}

21
ArduSub/defines.h
View File

@ -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

6
ArduSub/ekf_check.cpp
View File

@ -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;
}

142
ArduSub/events.cpp
View File

@ -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) {
if (should_disarm_on_failsafe()) {
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();
}
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();
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 {
// 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();
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
} else {
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
}
}
break;
}
// log the error to the dataflash
@ -109,50 +53,15 @@ 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) {
if (should_disarm_on_failsafe()) {
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();
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();
set_mode_land_with_pause(MODE_REASON_BATTERY_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();
// 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;
}
}
// set the low battery flag
@ -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();

6
ArduSub/fence.cpp
View File

@ -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);
}
}
}

25
ArduSub/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 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:

2
ArduSub/motors.cpp
View File

@ -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();

2
ArduSub/setup.cpp
View File

@ -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);
}

14
ArduSub/switches.cpp
View File

@ -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) {

2
ArduSub/takeoff.cpp
View File

@ -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;