zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: Support new AP_Vehicle::set_mode

master
Michael du Breuil 5 years ago committed by Randy Mackay
parent
6d7b196212
commit
b42b1c08c4
17 changed files with 88 additions and 110 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      ArduCopter/Copter.h
  2. 33
      ArduCopter/GCS_Mavlink.cpp
  3. 2
      ArduCopter/GCS_Mavlink.h
  4. 6
      ArduCopter/RC_Channel.cpp
  5. 14
      ArduCopter/avoidance_adsb.cpp
  6. 22
      ArduCopter/defines.h
  7. 6
      ArduCopter/ekf_check.cpp
  8. 32
      ArduCopter/events.cpp
  9. 18
      ArduCopter/fence.cpp
  10. 20
      ArduCopter/mode.cpp
  11. 4
      ArduCopter/mode.h
  12. 2
      ArduCopter/mode_auto.cpp
  13. 4
      ArduCopter/mode_brake.cpp
  14. 8
      ArduCopter/mode_flip.cpp
  15. 4
      ArduCopter/mode_land.cpp
  16. 6
      ArduCopter/mode_rtl.cpp
  17. 2
      ArduCopter/mode_throw.cpp

15
ArduCopter/Copter.h
Unescape View File

@ -388,10 +388,10 @@ private: @@ -388,10 +388,10 @@ private:
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
Mode::Number control_mode;
mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
ModeReason control_mode_reason = ModeReason::UNKNOWN;
Mode::Number prev_control_mode;
mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN;
ModeReason prev_control_mode_reason = ModeReason::UNKNOWN;
RCMapper rcmap;
@ -719,9 +719,9 @@ private: @@ -719,9 +719,9 @@ private:
void failsafe_terrain_set_status(bool data_ok);
void failsafe_terrain_on_event();
void gpsglitch_check();
void set_mode_RTL_or_land_with_pause(mode_reason_t reason);
void set_mode_SmartRTL_or_RTL(mode_reason_t reason);
void set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason);
void set_mode_RTL_or_land_with_pause(ModeReason reason);
void set_mode_SmartRTL_or_RTL(ModeReason reason);
void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
bool should_disarm_on_failsafe();
// failsafe.cpp
@ -782,12 +782,13 @@ private: @@ -782,12 +782,13 @@ private:
void log_init(void);
// mode.cpp
bool set_mode(Mode::Number mode, mode_reason_t reason);
bool set_mode(Mode::Number mode, ModeReason reason);
bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
void update_flight_mode();
void notify_flight_mode();
// mode_land.cpp
void set_mode_land_with_pause(mode_reason_t reason);
void set_mode_land_with_pause(ModeReason reason);
bool landing_with_GPS();
// motor_test.cpp

33
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -640,19 +640,19 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ @@ -640,19 +640,19 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
}
case MAV_CMD_NAV_LOITER_UNLIM:
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_GCS_COMMAND)) {
if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (!copter.set_mode(Mode::Number::RTL, MODE_REASON_GCS_COMMAND)) {
if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_LAND:
if (!copter.set_mode(Mode::Number::LAND, MODE_REASON_GCS_COMMAND)) {
if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
@ -704,7 +704,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ @@ -704,7 +704,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START:
if (copter.motors->armed() &&
copter.set_mode(Mode::Number::AUTO, MODE_REASON_GCS_COMMAND)) {
copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) {
copter.set_auto_armed(true);
if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) {
copter.mode_auto.mission.start_or_resume();
@ -800,8 +800,8 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ @@ -800,8 +800,8 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
}
// set mode to Loiter or fall back to AltHold
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_GCS_COMMAND)) {
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND);
if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
}
return MAV_RESULT_ACCEPTED;
}
@ -817,12 +817,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ @@ -817,12 +817,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
copter.arming.arm(AP_Arming::Method::MAVLINK);
} else if (copter.ap.land_complete) {
// if armed and landed, takeoff
if (copter.set_mode(Mode::Number::LOITER, MODE_REASON_GCS_COMMAND)) {
if (copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
copter.flightmode->do_user_takeoff(packet.param1*100, true);
}
} else {
// if flying, land
copter.set_mode(Mode::Number::LAND, MODE_REASON_GCS_COMMAND);
copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND);
}
return MAV_RESULT_ACCEPTED;
}
@ -844,13 +844,13 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ @@ -844,13 +844,13 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
if (!shot_mode) {
#if MODE_BRAKE_ENABLED == ENABLED
if (copter.set_mode(Mode::Number::BRAKE, MODE_REASON_GCS_COMMAND)) {
if (copter.set_mode(Mode::Number::BRAKE, ModeReason::GCS_COMMAND)) {
copter.mode_brake.timeout_to_loiter_ms(2500);
} else {
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND);
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
}
#else
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_GCS_COMMAND);
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
#endif
} else {
// SoloLink is expected to handle pause in shots
@ -1308,17 +1308,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_l @@ -1308,17 +1308,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_l
return result;
}
bool GCS_MAVLINK_Copter::set_mode(const uint8_t mode)
{
#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
if (copter.failsafe.radio) {
// don't allow mode changes while in radio failsafe
return false;
}
#endif
return copter.set_mode((Mode::Number)mode, MODE_REASON_GCS_COMMAND);
}
float GCS_MAVLINK_Copter::vfr_hud_alt() const
{
if (copter.g2.dev_options.get() & DevOptionVFR_HUDRelativeAlt) {

2
ArduCopter/GCS_Mavlink.h
Unescape View File

@ -18,8 +18,6 @@ protected: @@ -18,8 +18,6 @@ protected:
uint8_t sysid_my_gcs() const override;
bool sysid_enforce() const override;
bool set_mode(uint8_t mode) override;
bool params_ready() const override;
void send_banner() override;

6
ArduCopter/RC_Channel.cpp
Unescape View File

@ -20,7 +20,7 @@ void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos) @@ -20,7 +20,7 @@ void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos)
return;
}
if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), MODE_REASON_TX_COMMAND)) {
if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) {
// alert user to mode change failure
if (copter.ap.initialised) {
AP_Notify::events.user_mode_change_failed = 1;
@ -125,7 +125,7 @@ void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode, @@ -125,7 +125,7 @@ void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode,
switch(ch_flag) {
case HIGH: {
// engage mode (if not possible we remain in current flight mode)
const bool success = copter.set_mode(mode, MODE_REASON_TX_COMMAND);
const bool success = copter.set_mode(mode, ModeReason::RC_COMMAND);
if (copter.ap.initialised) {
if (success) {
AP_Notify::events.user_mode_change = 1;
@ -151,7 +151,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw @@ -151,7 +151,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
case AUX_FUNC::FLIP:
// flip if switch is on, positive throttle and we're actually flying
if (ch_flag == aux_switch_pos_t::HIGH) {
copter.set_mode(Mode::Number::FLIP, MODE_REASON_TX_COMMAND);
copter.set_mode(Mode::Number::FLIP, ModeReason::RC_COMMAND);
}
break;

14
ArduCopter/avoidance_adsb.cpp
Unescape View File

@ -44,7 +44,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O @@ -44,7 +44,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O
case MAV_COLLISION_ACTION_RTL:
// attempt to switch to RTL, if this fails (i.e. flying in manual mode with bad position) do nothing
if (failsafe_state_change) {
if (!copter.set_mode(Mode::Number::RTL, MODE_REASON_AVOIDANCE)) {
if (!copter.set_mode(Mode::Number::RTL, ModeReason::AVOIDANCE)) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
}
@ -53,7 +53,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O @@ -53,7 +53,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O
case MAV_COLLISION_ACTION_HOVER:
// attempt to switch to Loiter, if this fails (i.e. flying in manual mode with bad position) do nothing
if (failsafe_state_change) {
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_AVOIDANCE)) {
if (!copter.set_mode(Mode::Number::LOITER, ModeReason::AVOIDANCE)) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
}
@ -106,7 +106,7 @@ void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action) @@ -106,7 +106,7 @@ void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action)
LogErrorCode::ERROR_RESOLVED);
// restore flight mode if requested and user has not changed mode since
if (copter.control_mode_reason == MODE_REASON_AVOIDANCE) {
if (copter.control_mode_reason == ModeReason::AVOIDANCE) {
switch (recovery_action) {
case AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB:
@ -136,10 +136,10 @@ void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action) @@ -136,10 +136,10 @@ void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action)
void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(Mode::Number mode)
{
if (!copter.set_mode(mode, MODE_REASON_AVOIDANCE_RECOVERY)) {
if (!copter.set_mode(mode, ModeReason::AVOIDANCE_RECOVERY)) {
// on failure RTL or LAND
if (!copter.set_mode(Mode::Number::RTL, MODE_REASON_AVOIDANCE_RECOVERY)) {
copter.set_mode(Mode::Number::LAND, MODE_REASON_AVOIDANCE_RECOVERY);
if (!copter.set_mode(Mode::Number::RTL, ModeReason::AVOIDANCE_RECOVERY)) {
copter.set_mode(Mode::Number::LAND, ModeReason::AVOIDANCE_RECOVERY);
}
}
}
@ -149,7 +149,7 @@ bool AP_Avoidance_Copter::check_flightmode(bool allow_mode_change) @@ -149,7 +149,7 @@ bool AP_Avoidance_Copter::check_flightmode(bool allow_mode_change)
{
// ensure copter is in avoid_adsb mode
if (allow_mode_change && copter.control_mode != Mode::Number::AVOID_ADSB) {
if (!copter.set_mode(Mode::Number::AVOID_ADSB, MODE_REASON_AVOIDANCE)) {
if (!copter.set_mode(Mode::Number::AVOID_ADSB, ModeReason::AVOIDANCE)) {
// failed to set mode so exit immediately
return false;
}

22
ArduCopter/defines.h
Unescape View File

@ -30,28 +30,6 @@ enum autopilot_yaw_mode { @@ -30,28 +30,6 @@ enum autopilot_yaw_mode {
#define HIL_MODE_DISABLED 0
#define HIL_MODE_SENSORS 1
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,
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_BRAKE_TIMEOUT,
MODE_REASON_FLIP_COMPLETE,
MODE_REASON_AVOIDANCE,
MODE_REASON_AVOIDANCE_RECOVERY,
MODE_REASON_THROW_COMPLETE,
MODE_REASON_TERMINATE,
MODE_REASON_TMODE,
};
// Tuning enumeration
enum tuning_func {
TUNING_NONE = 0, //

6
ArduCopter/ekf_check.cpp
Unescape View File

@ -158,14 +158,14 @@ void Copter::failsafe_ekf_event() @@ -158,14 +158,14 @@ void Copter::failsafe_ekf_event()
switch (g.fs_ekf_action) {
case FS_EKF_ACTION_ALTHOLD:
// AltHold
if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) {
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)) {
set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
}
break;
case FS_EKF_ACTION_LAND:
case FS_EKF_ACTION_LAND_EVEN_STABILIZE:
default:
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
break;
}
}

32
ArduCopter/events.cpp
Unescape View File

@ -22,15 +22,15 @@ void Copter::failsafe_radio_on_event() @@ -22,15 +22,15 @@ void Copter::failsafe_radio_on_event()
// continue landing or other high priority failsafes
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_RADIO_FAILSAFE);
set_mode_SmartRTL_or_RTL(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
set_mode_SmartRTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else { // g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
set_mode_land_with_pause(ModeReason::RADIO_FAILSAFE);
}
}
}
@ -61,16 +61,16 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) @@ -61,16 +61,16 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
case Failsafe_Action_None:
return;
case Failsafe_Action_Land:
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
set_mode_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_RTL:
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
set_mode_RTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL:
set_mode_SmartRTL_or_RTL(MODE_REASON_BATTERY_FAILSAFE);
set_mode_SmartRTL_or_RTL(ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Land:
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
set_mode_SmartRTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED
@ -138,11 +138,11 @@ void Copter::failsafe_gcs_check() @@ -138,11 +138,11 @@ void Copter::failsafe_gcs_check()
g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_GCS_FAILSAFE);
set_mode_SmartRTL_or_RTL(ModeReason::GCS_FAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
set_mode_SmartRTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
} else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL
set_mode_RTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
set_mode_RTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
}
}
}
@ -210,7 +210,7 @@ void Copter::failsafe_terrain_on_event() @@ -210,7 +210,7 @@ void Copter::failsafe_terrain_on_event()
mode_rtl.restart_without_terrain();
#endif
} else {
set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE);
set_mode_RTL_or_land_with_pause(ModeReason::TERRAIN_FAILSAFE);
}
}
@ -236,7 +236,7 @@ void Copter::gpsglitch_check() @@ -236,7 +236,7 @@ void Copter::gpsglitch_check()
// 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(mode_reason_t reason)
void Copter::set_mode_RTL_or_land_with_pause(ModeReason reason)
{
// attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(Mode::Number::RTL, reason)) {
@ -250,7 +250,7 @@ void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason) @@ -250,7 +250,7 @@ void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason)
// set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_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_SmartRTL_or_land_with_pause(mode_reason_t reason)
void Copter::set_mode_SmartRTL_or_land_with_pause(ModeReason reason)
{
// attempt to switch to SMART_RTL, if this failed then switch to Land
if (!set_mode(Mode::Number::SMART_RTL, reason)) {
@ -263,7 +263,7 @@ void Copter::set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason) @@ -263,7 +263,7 @@ void Copter::set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason)
// set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or 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_SmartRTL_or_RTL(mode_reason_t reason)
void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
{
// attempt to switch to SmartRTL, if this failed then attempt to RTL
// if that fails, then land

18
ArduCopter/fence.cpp
Unescape View File

@ -36,32 +36,32 @@ void Copter::fence_check() @@ -36,32 +36,32 @@ void Copter::fence_check()
// if more than 100m outside the fence just force a land
if (fence.get_breach_distance(new_breaches) > AC_FENCE_GIVE_UP_DISTANCE) {
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH);
set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
} else {
switch (fence_act) {
case AC_FENCE_ACTION_RTL_AND_LAND:
default:
// switch to RTL, if that fails then Land
if (!set_mode(Mode::Number::RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH);
if (!set_mode(Mode::Number::RTL, ModeReason::FENCE_BREACHED)) {
set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
}
break;
case AC_FENCE_ACTION_ALWAYS_LAND:
// if always land option mode is specified, land
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH);
set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
break;
case AC_FENCE_ACTION_SMART_RTL:
// Try SmartRTL, if that fails, RTL, if that fails Land
if (!set_mode(Mode::Number::SMART_RTL, MODE_REASON_FENCE_BREACH)) {
if (!set_mode(Mode::Number::RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH);
if (!set_mode(Mode::Number::SMART_RTL, ModeReason::FENCE_BREACHED)) {
if (!set_mode(Mode::Number::RTL, ModeReason::FENCE_BREACHED)) {
set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
}
}
break;
case AC_FENCE_ACTION_BRAKE:
// Try Brake, if that fails Land
if (!set_mode(Mode::Number::BRAKE, MODE_REASON_FENCE_BREACH)) {
set_mode(Mode::Number::LAND, MODE_REASON_FENCE_BREACH);
if (!set_mode(Mode::Number::BRAKE, ModeReason::FENCE_BREACHED)) {
set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
}
break;
}

20
ArduCopter/mode.cpp
Unescape View File

@ -177,7 +177,7 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode) @@ -177,7 +177,7 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully 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(Mode::Number mode, mode_reason_t reason)
bool Copter::set_mode(Mode::Number mode, ModeReason reason)
{
// return immediately if we are already in the desired mode
@ -273,6 +273,18 @@ bool Copter::set_mode(Mode::Number mode, mode_reason_t reason) @@ -273,6 +273,18 @@ bool Copter::set_mode(Mode::Number mode, mode_reason_t reason)
return true;
}
bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason)
{
static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
if (reason == ModeReason::GCS_COMMAND && copter.failsafe.radio) {
// don't allow mode changes while in radio failsafe
return false;
}
#endif
return copter.set_mode(static_cast<Mode::Number>(new_mode), ModeReason::GCS_COMMAND);
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void Copter::update_flight_mode()
@ -542,8 +554,8 @@ void Mode::land_run_horizontal_control() @@ -542,8 +554,8 @@ void Mode::land_run_horizontal_control()
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
copter.Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
if (!set_mode(Mode::Number::LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
set_mode(Mode::Number::ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
}
}
@ -738,7 +750,7 @@ void Mode::update_simple_mode(void) { @@ -738,7 +750,7 @@ void Mode::update_simple_mode(void) {
copter.update_simple_mode();
}
bool Mode::set_mode(Mode::Number mode, mode_reason_t reason)
bool Mode::set_mode(Mode::Number mode, ModeReason reason)
{
return copter.set_mode(mode, reason);
}

4
ArduCopter/mode.h
Unescape View File

@ -12,7 +12,7 @@ class Mode { @@ -12,7 +12,7 @@ class Mode {
public:
// Auto Pilot Modes enumeration
enum class Number {
enum class Number : uint8_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
@ -238,7 +238,7 @@ public: @@ -238,7 +238,7 @@ public:
float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle(void);
void update_simple_mode(void);
bool set_mode(Mode::Number mode, mode_reason_t reason);
bool set_mode(Mode::Number mode, ModeReason reason);
void set_land_complete(bool b);
GCS_Copter &gcs();
void Log_Write_Event(Log_Event id);

2
ArduCopter/mode_auto.cpp
Unescape View File

@ -539,7 +539,7 @@ void ModeAuto::exit_mission() @@ -539,7 +539,7 @@ void ModeAuto::exit_mission()
if (!copter.ap.land_complete) {
// try to enter loiter but if that fails land
if (!loiter_start()) {
set_mode(Mode::Number::LAND, MODE_REASON_MISSION_END);
set_mode(Mode::Number::LAND, ModeReason::MISSION_END);
}
} else {
// if we've landed it's safe to disarm

4
ArduCopter/mode_brake.cpp
Unescape View File

@ -63,8 +63,8 @@ void ModeBrake::run() @@ -63,8 +63,8 @@ void ModeBrake::run()
pos_control->update_z_controller();
if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {
if (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
if (!copter.set_mode(Mode::Number::LOITER, ModeReason::BRAKE_TIMEOUT)) {
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT);
}
}
}

8
ArduCopter/mode_flip.cpp
Unescape View File

@ -192,9 +192,9 @@ void ModeFlip::run() @@ -192,9 +192,9 @@ void ModeFlip::run()
// check for successful recovery
if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode
if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
// this should never happen but just in case
copter.set_mode(Mode::Number::STABILIZE, MODE_REASON_UNKNOWN);
copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
}
// log successful completion
Log_Write_Event(DATA_FLIP_END);
@ -203,9 +203,9 @@ void ModeFlip::run() @@ -203,9 +203,9 @@ void ModeFlip::run()
}
case FlipState::Abandon:
// restore original flight mode
if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
// this should never happen but just in case
copter.set_mode(Mode::Number::STABILIZE, MODE_REASON_UNKNOWN);
copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
}
// log abandoning flip
AP::logger().Write_Error(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED);

4
ArduCopter/mode_land.cpp
Unescape View File

@ -93,7 +93,7 @@ void ModeLand::nogps_run() @@ -93,7 +93,7 @@ void ModeLand::nogps_run()
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
}
if (g.land_repositioning) {
@ -145,7 +145,7 @@ void ModeLand::do_not_use_GPS() @@ -145,7 +145,7 @@ void ModeLand::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(mode_reason_t reason)
void Copter::set_mode_land_with_pause(ModeReason reason)
{
set_mode(Mode::Number::LAND, reason);
land_pause = true;

6
ArduCopter/mode_rtl.cpp
Unescape View File

@ -119,7 +119,7 @@ void ModeRTL::climb_start() @@ -119,7 +119,7 @@ void ModeRTL::climb_start()
if (!wp_nav->set_wp_destination(rtl_path.climb_target)) {
// this should not happen because rtl_build_path will have checked terrain data was available
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
copter.set_mode(Mode::Number::LAND, MODE_REASON_TERRAIN_FAILSAFE);
copter.set_mode(Mode::Number::LAND, ModeReason::TERRAIN_FAILSAFE);
return;
}
wp_nav->set_fast_waypoint(true);
@ -287,8 +287,8 @@ void ModeRTL::descent_run() @@ -287,8 +287,8 @@ void ModeRTL::descent_run()
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.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 (!copter.set_mode(Mode::Number::LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
if (!copter.set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
}
}

2
ArduCopter/mode_throw.cpp
Unescape View File

@ -92,7 +92,7 @@ void ModeThrow::run() @@ -92,7 +92,7 @@ void ModeThrow::run()
case Mode::Number::LAND:
case Mode::Number::BRAKE:
case Mode::Number::LOITER:
set_mode((Mode::Number)g2.throw_nextmode.get(), MODE_REASON_THROW_COMPLETE);
set_mode((Mode::Number)g2.throw_nextmode.get(), ModeReason::THROW_COMPLETE);
break;
default:
// do nothing

Write Preview
Loading…
Cancel
Save