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

Plane: Support new AP_Vehicle::set_mode

master
Michael du Breuil 5 years ago committed by Randy Mackay
parent
b42b1c08c4
commit
ee96ec7f0d
17 changed files with 79 additions and 103 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      ArduPlane/ArduPlane.cpp
  2. 22
      ArduPlane/GCS_Mavlink.cpp
  3. 2
      ArduPlane/GCS_Mavlink.h
  4. 17
      ArduPlane/Plane.h
  5. 2
      ArduPlane/RC_Channel.cpp
  6. 2
      ArduPlane/afs_plane.cpp
  7. 16
      ArduPlane/avoidance_adsb.cpp
  8. 4
      ArduPlane/commands_logic.cpp
  9. 2
      ArduPlane/control_modes.cpp
  10. 22
      ArduPlane/defines.h
  11. 22
      ArduPlane/events.cpp
  12. 6
      ArduPlane/geofence.cpp
  13. 2
      ArduPlane/mode.h
  14. 2
      ArduPlane/mode_auto.cpp
  15. 6
      ArduPlane/quadplane.cpp
  16. 10
      ArduPlane/soaring.cpp
  17. 39
      ArduPlane/system.cpp

6
ArduPlane/ArduPlane.cpp
Unescape View File

@ -453,7 +453,7 @@ void Plane::update_navigation()
are within the maximum of the stopping distance and the are within the maximum of the stopping distance and the
RTL_RADIUS RTL_RADIUS
*/ */
set_mode(mode_qrtl, MODE_REASON_UNKNOWN); set_mode(mode_qrtl, ModeReason::UNKNOWN);
break; break;
} else if (g.rtl_autoland == 1 && } else if (g.rtl_autoland == 1 &&
!auto_state.checked_for_autoland && !auto_state.checked_for_autoland &&
@ -462,7 +462,7 @@ void Plane::update_navigation()
// we've reached the RTL point, see if we have a landing sequence // we've reached the RTL point, see if we have a landing sequence
if (mission.jump_to_landing_sequence()) { if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO // switch from RTL -> AUTO
set_mode(mode_auto, MODE_REASON_UNKNOWN); set_mode(mode_auto, ModeReason::UNKNOWN);
} }
// prevent running the expensive jump_to_landing_sequence // prevent running the expensive jump_to_landing_sequence
@ -474,7 +474,7 @@ void Plane::update_navigation()
// Go directly to the landing sequence // Go directly to the landing sequence
if (mission.jump_to_landing_sequence()) { if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO // switch from RTL -> AUTO
set_mode(mode_auto, MODE_REASON_UNKNOWN); set_mode(mode_auto, ModeReason::UNKNOWN);
} }
// prevent running the expensive jump_to_landing_sequence // prevent running the expensive jump_to_landing_sequence

22
ArduPlane/GCS_Mavlink.cpp
Unescape View File

@ -774,7 +774,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
// location is valid load and set // location is valid load and set
if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
(plane.control_mode == &plane.mode_guided)) { (plane.control_mode == &plane.mode_guided)) {
plane.set_mode(plane.mode_guided, MODE_REASON_GCS_COMMAND); plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
plane.guided_WP_loc = requested_position; plane.guided_WP_loc = requested_position;
// add home alt if needed // add home alt if needed
@ -827,11 +827,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
} }
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
plane.set_mode(plane.mode_loiter, MODE_REASON_GCS_COMMAND); plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND);
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: case MAV_CMD_NAV_RETURN_TO_LAUNCH:
plane.set_mode(plane.mode_rtl, MODE_REASON_GCS_COMMAND); plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND);
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_TAKEOFF: { case MAV_CMD_NAV_TAKEOFF: {
@ -845,13 +845,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
} }
case MAV_CMD_MISSION_START: case MAV_CMD_MISSION_START:
plane.set_mode(plane.mode_auto, MODE_REASON_GCS_COMMAND); plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
case MAV_CMD_DO_LAND_START: case MAV_CMD_DO_LAND_START:
// attempt to switch to next DO_LAND_START command in the mission // attempt to switch to next DO_LAND_START command in the mission
if (plane.mission.jump_to_landing_sequence()) { if (plane.mission.jump_to_landing_sequence()) {
plane.set_mode(plane.mode_auto, MODE_REASON_UNKNOWN); plane.set_mode(plane.mode_auto, ModeReason::UNKNOWN);
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
@ -1386,18 +1386,6 @@ void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, const ma
} }
} }
/*
set_mode() wrapper for MAVLink SET_MODE
*/
bool GCS_MAVLINK_Plane::set_mode(const uint8_t mode)
{
Mode *new_mode = plane.mode_from_mode_num((enum Mode::Number)mode);
if (new_mode == nullptr) {
return false;
}
return plane.set_mode(*new_mode, MODE_REASON_GCS_COMMAND);
}
uint64_t GCS_MAVLINK_Plane::capabilities() const uint64_t GCS_MAVLINK_Plane::capabilities() const
{ {
return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT | return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |

2
ArduPlane/GCS_Mavlink.h
Unescape View File

@ -19,8 +19,6 @@ protected:
uint8_t sysid_my_gcs() const override; uint8_t sysid_my_gcs() const override;
bool sysid_enforce() const override; bool sysid_enforce() const override;
bool set_mode(uint8_t mode) override;
MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override;

17
ArduPlane/Plane.h
Unescape View File

@ -294,9 +294,9 @@ private:
// This is the state of the flight control system // This is the state of the flight control system
// There are multiple states defined such as MANUAL, FBW-A, AUTO // There are multiple states defined such as MANUAL, FBW-A, AUTO
Mode *control_mode = &mode_initializing; Mode *control_mode = &mode_initializing;
mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN; ModeReason control_mode_reason = ModeReason::UNKNOWN;
Mode *previous_mode = &mode_initializing; Mode *previous_mode = &mode_initializing;
mode_reason_t previous_mode_reason = MODE_REASON_UNKNOWN; ModeReason previous_mode_reason = ModeReason::UNKNOWN;
// time of last mode change // time of last mode change
uint32_t last_mode_change_ms; uint32_t last_mode_change_ms;
@ -855,10 +855,10 @@ private:
void autotune_restore(void); void autotune_restore(void);
void autotune_enable(bool enable); void autotune_enable(bool enable);
bool fly_inverted(void); bool fly_inverted(void);
void failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason); void failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason);
void failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason); void failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason);
void failsafe_short_off_event(mode_reason_t reason); void failsafe_short_off_event(ModeReason reason);
void failsafe_long_off_event(mode_reason_t reason); void failsafe_long_off_event(ModeReason reason);
void handle_battery_failsafe(const char* type_str, const int8_t action); void handle_battery_failsafe(const char* type_str, const int8_t action);
uint8_t max_fencepoints(void) const; uint8_t max_fencepoints(void) const;
Vector2l get_fence_point_with_index(uint8_t i) const; Vector2l get_fence_point_with_index(uint8_t i) const;
@ -906,8 +906,9 @@ private:
void rpm_update(void); void rpm_update(void);
void init_ardupilot(); void init_ardupilot();
void startup_ground(void); void startup_ground(void);
bool set_mode(Mode& new_mode, const mode_reason_t reason); bool set_mode(Mode& new_mode, const ModeReason reason);
bool set_mode_by_number(const Mode::Number new_mode_number, const mode_reason_t reason); bool set_mode(const uint8_t mode, const ModeReason reason) override;
bool set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason);
Mode *mode_from_mode_num(const enum Mode::Number num); Mode *mode_from_mode_num(const enum Mode::Number num);
void check_long_failsafe(); void check_long_failsafe();
void check_short_failsafe(); void check_short_failsafe();

2
ArduPlane/RC_Channel.cpp
Unescape View File

@ -33,7 +33,7 @@ void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number,
switch(ch_flag) { switch(ch_flag) {
case HIGH: { case HIGH: {
// engage mode (if not possible we remain in current flight mode) // engage mode (if not possible we remain in current flight mode)
const bool success = plane.set_mode_by_number(number, MODE_REASON_TX_COMMAND); const bool success = plane.set_mode_by_number(number, ModeReason::RC_COMMAND);
if (plane.control_mode != &plane.mode_initializing) { if (plane.control_mode != &plane.mode_initializing) {
if (success) { if (success) {
AP_Notify::events.user_mode_change = 1; AP_Notify::events.user_mode_change = 1;

2
ArduPlane/afs_plane.cpp
Unescape View File

@ -18,7 +18,7 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
{ {
if (plane.quadplane.available() && _terminate_action == TERMINATE_ACTION_LAND) { if (plane.quadplane.available() && _terminate_action == TERMINATE_ACTION_LAND) {
// perform a VTOL landing // perform a VTOL landing
plane.set_mode(plane.mode_qland, MODE_REASON_FENCE_BREACH); plane.set_mode(plane.mode_qland, ModeReason::FENCE_BREACHED);
return; return;
} }

16
ArduPlane/avoidance_adsb.cpp
Unescape View File

@ -36,16 +36,16 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
case MAV_COLLISION_ACTION_RTL: case MAV_COLLISION_ACTION_RTL:
if (failsafe_state_change) { if (failsafe_state_change) {
plane.set_mode(plane.mode_rtl, MODE_REASON_AVOIDANCE); plane.set_mode(plane.mode_rtl, ModeReason::AVOIDANCE);
} }
break; break;
case MAV_COLLISION_ACTION_HOVER: case MAV_COLLISION_ACTION_HOVER:
if (failsafe_state_change) { if (failsafe_state_change) {
if (plane.quadplane.is_flying()) { if (plane.quadplane.is_flying()) {
plane.set_mode(plane.mode_qloiter, MODE_REASON_AVOIDANCE); plane.set_mode(plane.mode_qloiter, ModeReason::AVOIDANCE);
} else { } else {
plane.set_mode(plane.mode_loiter, MODE_REASON_AVOIDANCE); plane.set_mode(plane.mode_loiter, ModeReason::AVOIDANCE);
} }
} }
break; break;
@ -105,7 +105,7 @@ void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action)
gcs().send_text(MAV_SEVERITY_INFO, "Avoid: Resuming with action: %d", recovery_action); gcs().send_text(MAV_SEVERITY_INFO, "Avoid: Resuming with action: %d", recovery_action);
// restore flight mode if requested and user has not changed mode since // restore flight mode if requested and user has not changed mode since
if (plane.control_mode_reason == MODE_REASON_AVOIDANCE) { if (plane.control_mode_reason == ModeReason::AVOIDANCE) {
switch (recovery_action) { switch (recovery_action) {
case AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB: case AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB:
@ -113,16 +113,16 @@ void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action)
break; break;
case AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE: case AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE:
plane.set_mode_by_number(prev_control_mode_number, MODE_REASON_AVOIDANCE_RECOVERY); plane.set_mode_by_number(prev_control_mode_number, ModeReason::AVOIDANCE_RECOVERY);
break; break;
case AP_AVOIDANCE_RECOVERY_RTL: case AP_AVOIDANCE_RECOVERY_RTL:
plane.set_mode(plane.mode_rtl, MODE_REASON_AVOIDANCE_RECOVERY); plane.set_mode(plane.mode_rtl, ModeReason::AVOIDANCE_RECOVERY);
break; break;
case AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER: case AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER:
if (prev_control_mode_number == Mode::Number::AUTO) { if (prev_control_mode_number == Mode::Number::AUTO) {
plane.set_mode(plane.mode_auto, MODE_REASON_AVOIDANCE_RECOVERY); plane.set_mode(plane.mode_auto, ModeReason::AVOIDANCE_RECOVERY);
} }
// else do nothing, same as AP_AVOIDANCE_RECOVERY_LOITER // else do nothing, same as AP_AVOIDANCE_RECOVERY_LOITER
break; break;
@ -139,7 +139,7 @@ bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change)
{ {
// ensure plane is in avoid_adsb mode // ensure plane is in avoid_adsb mode
if (allow_mode_change && plane.control_mode != &plane.mode_avoidADSB) { if (allow_mode_change && plane.control_mode != &plane.mode_avoidADSB) {
plane.set_mode(plane.mode_avoidADSB, MODE_REASON_AVOIDANCE); plane.set_mode(plane.mode_avoidADSB, ModeReason::AVOIDANCE);
} }
// check flight mode // check flight mode

4
ArduPlane/commands_logic.cpp
Unescape View File

@ -75,7 +75,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
break; break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: case MAV_CMD_NAV_RETURN_TO_LAUNCH:
set_mode(mode_rtl, MODE_REASON_UNKNOWN); set_mode(mode_rtl, ModeReason::UNKNOWN);
break; break;
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
@ -962,7 +962,7 @@ bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
void Plane::exit_mission_callback() void Plane::exit_mission_callback()
{ {
if (control_mode == &mode_auto) { if (control_mode == &mode_auto) {
set_mode(mode_rtl, MODE_REASON_MISSION_END); set_mode(mode_rtl, ModeReason::MISSION_END);
gcs().send_text(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL"); gcs().send_text(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL");
} }
} }

2
ArduPlane/control_modes.cpp
Unescape View File

@ -116,7 +116,7 @@ void Plane::read_control_switch()
return; return;
} }
set_mode_by_number((enum Mode::Number)flight_modes[switchPosition].get(), MODE_REASON_TX_COMMAND); set_mode_by_number((enum Mode::Number)flight_modes[switchPosition].get(), ModeReason::RC_COMMAND);
oldSwitchPosition = switchPosition; oldSwitchPosition = switchPosition;
} }

22
ArduPlane/defines.h
Unescape View File

@ -45,28 +45,6 @@ enum failsafe_action_long {
FS_ACTION_LONG_PARACHUTE = 3, FS_ACTION_LONG_PARACHUTE = 3,
}; };
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_FENCE_BREACH,
MODE_REASON_AVOIDANCE,
MODE_REASON_AVOIDANCE_RECOVERY,
MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING,
MODE_REASON_SOARING_THERMAL_DETECTED,
MODE_REASON_SOARING_IN_THERMAL,
MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED,
MODE_REASON_VTOL_FAILED_TRANSITION,
MODE_REASON_UNAVAILABLE,
MODE_REASON_VTOL_FAILED_TAKEOFF,
};
// type of stick mixing enabled // type of stick mixing enabled
enum StickMixing { enum StickMixing {
STICK_MIXING_DISABLED = 0, STICK_MIXING_DISABLED = 0,

22
ArduPlane/events.cpp
Unescape View File

@ -1,11 +1,11 @@
#include "Plane.h" #include "Plane.h"
void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason) void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason)
{ {
// This is how to handle a short loss of control signal failsafe. // This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype; failsafe.state = fstype;
failsafe.short_timer_ms = millis(); failsafe.short_timer_ms = millis();
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, reason); gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, static_cast<unsigned>(reason));
switch (control_mode->mode_number()) switch (control_mode->mode_number())
{ {
case Mode::Number::MANUAL: case Mode::Number::MANUAL:
@ -65,10 +65,10 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number()); gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number());
} }
void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason) void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
{ {
// This is how to handle a long loss of control signal failsafe. // This is how to handle a long loss of control signal failsafe.
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on: type=%u/reason=%u", fstype, reason); gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on: type=%u/reason=%u", fstype, static_cast<unsigned>(reason));
// If the GCS is locked up we allow control to revert to RC // If the GCS is locked up we allow control to revert to RC
RC_Channels::clear_overrides(); RC_Channels::clear_overrides();
failsafe.state = fstype; failsafe.state = fstype;
@ -131,10 +131,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t rea
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number()); gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number());
} }
void Plane::failsafe_short_off_event(mode_reason_t reason) void Plane::failsafe_short_off_event(ModeReason reason)
{ {
// We're back in radio contact // We're back in radio contact
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", reason); gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", static_cast<unsigned>(reason));
failsafe.state = FAILSAFE_NONE; failsafe.state = FAILSAFE_NONE;
// re-read the switch so we can return to our preferred mode // re-read the switch so we can return to our preferred mode
@ -145,10 +145,10 @@ void Plane::failsafe_short_off_event(mode_reason_t reason)
} }
} }
void Plane::failsafe_long_off_event(mode_reason_t reason) void Plane::failsafe_long_off_event(ModeReason reason)
{ {
// We're back in radio contact // We're back in radio contact
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event off: reason=%u", reason); gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event off: reason=%u", static_cast<unsigned>(reason));
failsafe.state = FAILSAFE_NONE; failsafe.state = FAILSAFE_NONE;
} }
@ -157,7 +157,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
switch ((Failsafe_Action)action) { switch ((Failsafe_Action)action) {
case Failsafe_Action_QLand: case Failsafe_Action_QLand:
if (quadplane.available()) { if (quadplane.available()) {
plane.set_mode(mode_qland, MODE_REASON_BATTERY_FAILSAFE); plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
break; break;
} }
FALLTHROUGH; FALLTHROUGH;
@ -165,7 +165,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland) { if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland) {
// never stop a landing if we were already committed // never stop a landing if we were already committed
if (plane.mission.jump_to_landing_sequence()) { if (plane.mission.jump_to_landing_sequence()) {
plane.set_mode(mode_auto, MODE_REASON_BATTERY_FAILSAFE); plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE);
break; break;
} }
} }
@ -173,7 +173,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
case Failsafe_Action_RTL: case Failsafe_Action_RTL:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland ) { if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland ) {
// never stop a landing if we were already committed // never stop a landing if we were already committed
set_mode(mode_rtl, MODE_REASON_BATTERY_FAILSAFE); set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE);
aparm.throttle_cruise.load(); aparm.throttle_cruise.load();
} }
break; break;

6
ArduPlane/geofence.cpp
Unescape View File

@ -336,7 +336,7 @@ void Plane::geofence_check(bool altitude_check_only)
guided_WP_loc.lat == geofence_state->guided_lat && guided_WP_loc.lat == geofence_state->guided_lat &&
guided_WP_loc.lng == geofence_state->guided_lng) { guided_WP_loc.lng == geofence_state->guided_lng) {
geofence_state->old_switch_position = 254; geofence_state->old_switch_position = 254;
set_mode(*previous_mode, MODE_REASON_GCS_COMMAND); set_mode(*previous_mode, ModeReason::GCS_COMMAND);
} }
return; return;
} }
@ -424,9 +424,9 @@ void Plane::geofence_check(bool altitude_check_only)
int8_t saved_auto_trim = g.auto_trim; int8_t saved_auto_trim = g.auto_trim;
g.auto_trim.set(0); g.auto_trim.set(0);
if (g.fence_action == FENCE_ACTION_RTL) { if (g.fence_action == FENCE_ACTION_RTL) {
set_mode(mode_rtl, MODE_REASON_FENCE_BREACH); set_mode(mode_rtl, ModeReason::FENCE_BREACHED);
} else { } else {
set_mode(mode_guided, MODE_REASON_FENCE_BREACH); set_mode(mode_guided, ModeReason::FENCE_BREACHED);
} }
g.auto_trim.set(saved_auto_trim); g.auto_trim.set(saved_auto_trim);

2
ArduPlane/mode.h
Unescape View File

@ -14,7 +14,7 @@ public:
// Auto Pilot modes // Auto Pilot modes
// ---------------- // ----------------
enum Number { enum Number : uint8_t {
MANUAL = 0, MANUAL = 0,
CIRCLE = 1, CIRCLE = 1,
STABILIZE = 2, STABILIZE = 2,

2
ArduPlane/mode_auto.cpp
Unescape View File

@ -49,7 +49,7 @@ void ModeAuto::update()
if (plane.mission.state() != AP_Mission::MISSION_RUNNING) { if (plane.mission.state() != AP_Mission::MISSION_RUNNING) {
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if: // this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point // restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
plane.set_mode(plane.mode_rtl, MODE_REASON_MISSION_END); plane.set_mode(plane.mode_rtl, ModeReason::MISSION_END);
gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission"); gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return; return;
} }

6
ArduPlane/quadplane.cpp
Unescape View File

@ -1456,7 +1456,7 @@ void QuadPlane::update_transition(void)
(transition_failure > 0) && (transition_failure > 0) &&
((now - transition_start_ms) > ((uint32_t)transition_failure * 1000))) { ((now - transition_start_ms) > ((uint32_t)transition_failure * 1000))) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit");
plane.set_mode(plane.mode_qland, MODE_REASON_VTOL_FAILED_TRANSITION); plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TRANSITION);
} }
float aspeed; float aspeed;
@ -2576,13 +2576,13 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
// check for failure conditions // check for failure conditions
if (is_positive(takeoff_failure_scalar) && ((now - takeoff_start_time_ms) > takeoff_time_limit_ms)) { if (is_positive(takeoff_failure_scalar) && ((now - takeoff_start_time_ms) > takeoff_time_limit_ms)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff within time limit"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff within time limit");
plane.set_mode(plane.mode_qland, MODE_REASON_VTOL_FAILED_TAKEOFF); plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF);
return false; return false;
} }
if (is_positive(maximum_takeoff_airspeed) && (plane.airspeed.get_airspeed() > maximum_takeoff_airspeed)) { if (is_positive(maximum_takeoff_airspeed) && (plane.airspeed.get_airspeed() > maximum_takeoff_airspeed)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff, excessive wind"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff, excessive wind");
plane.set_mode(plane.mode_qland, MODE_REASON_VTOL_FAILED_TAKEOFF); plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF);
return false; return false;
} }

10
ArduPlane/soaring.cpp
Unescape View File

@ -24,7 +24,7 @@ void Plane::update_soaring() {
case Mode::Number::CRUISE: case Mode::Number::CRUISE:
if (!g2.soaring_controller.suppress_throttle()) { if (!g2.soaring_controller.suppress_throttle()) {
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: forcing RTL"); gcs().send_text(MAV_SEVERITY_INFO, "Soaring: forcing RTL");
set_mode(mode_rtl, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING); set_mode(mode_rtl, ModeReason::SOARING_FBW_B_WITH_MOTOR_RUNNING);
} }
break; break;
case Mode::Number::LOITER: case Mode::Number::LOITER:
@ -51,7 +51,7 @@ void Plane::update_soaring() {
if (g2.soaring_controller.check_thermal_criteria()) { if (g2.soaring_controller.check_thermal_criteria()) {
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter"); gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter");
set_mode(mode_loiter, MODE_REASON_SOARING_THERMAL_DETECTED); set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED);
} }
break; break;
@ -64,14 +64,14 @@ void Plane::update_soaring() {
switch (previous_mode->mode_number()) { switch (previous_mode->mode_number()) {
case Mode::Number::FLY_BY_WIRE_B: case Mode::Number::FLY_BY_WIRE_B:
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL"); gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL");
set_mode(mode_rtl, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); set_mode(mode_rtl, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
break; break;
case Mode::Number::CRUISE: { case Mode::Number::CRUISE: {
// return to cruise with old ground course // return to cruise with old ground course
CruiseState cruise = cruise_state; CruiseState cruise = cruise_state;
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE"); gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE");
set_mode(mode_cruise, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
cruise_state = cruise; cruise_state = cruise;
set_target_altitude_current(); set_target_altitude_current();
break; break;
@ -79,7 +79,7 @@ void Plane::update_soaring() {
case Mode::Number::AUTO: case Mode::Number::AUTO:
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO"); gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO");
set_mode(mode_auto, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
break; break;
default: default:

39
ArduPlane/system.cpp
Unescape View File

@ -163,7 +163,7 @@ void Plane::init_ardupilot()
// choose the nav controller // choose the nav controller
set_nav_controller(); set_nav_controller();
set_mode_by_number((enum Mode::Number)g.initial_mode.get(), MODE_REASON_UNKNOWN); set_mode_by_number((enum Mode::Number)g.initial_mode.get(), ModeReason::UNKNOWN);
// set the correct flight mode // set the correct flight mode
// --------------------------- // ---------------------------
@ -194,7 +194,7 @@ void Plane::init_ardupilot()
//******************************************************************************** //********************************************************************************
void Plane::startup_ground(void) void Plane::startup_ground(void)
{ {
set_mode(mode_initializing, MODE_REASON_UNKNOWN); set_mode(mode_initializing, ModeReason::UNKNOWN);
#if (GROUND_START_DELAY > 0) #if (GROUND_START_DELAY > 0)
gcs().send_text(MAV_SEVERITY_NOTICE,"Ground start with delay"); gcs().send_text(MAV_SEVERITY_NOTICE,"Ground start with delay");
@ -241,7 +241,7 @@ void Plane::startup_ground(void)
} }
bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason) bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
{ {
if (control_mode == &new_mode) { if (control_mode == &new_mode) {
// don't switch modes if we are already in the correct mode. // don't switch modes if we are already in the correct mode.
@ -251,7 +251,7 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason)
#if !QAUTOTUNE_ENABLED #if !QAUTOTUNE_ENABLED
if (&new_mode == &plane.mode_qautotune) { if (&new_mode == &plane.mode_qautotune) {
gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled"); gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled");
set_mode(plane.mode_qhover, MODE_REASON_UNAVAILABLE); set_mode(plane.mode_qhover, ModeReason::UNAVAILABLE);
return false; return false;
} }
#endif #endif
@ -259,7 +259,7 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason)
// backup current control_mode and previous_mode // backup current control_mode and previous_mode
Mode &old_previous_mode = *previous_mode; Mode &old_previous_mode = *previous_mode;
Mode &old_mode = *control_mode; Mode &old_mode = *control_mode;
const mode_reason_t previous_mode_reason_backup = previous_mode_reason; const ModeReason previous_mode_reason_backup = previous_mode_reason;
// update control_mode assuming success // update control_mode assuming success
// TODO: move these to be after enter() once start_command_callback() no longer checks control_mode // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
@ -309,7 +309,18 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason)
return true; return true;
} }
bool Plane::set_mode_by_number(const Mode::Number new_mode_number, const mode_reason_t reason) bool Plane::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");
Mode *mode = plane.mode_from_mode_num(static_cast<Mode::Number>(new_mode));
if (mode == nullptr) {
gcs().send_text(MAV_SEVERITY_INFO, "Error: invalid mode number: %u", (unsigned)new_mode);
return false;
}
return set_mode(*mode, reason);
}
bool Plane::set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason)
{ {
Mode *new_mode = plane.mode_from_mode_num(new_mode_number); Mode *new_mode = plane.mode_from_mode_num(new_mode_number);
if (new_mode == nullptr) { if (new_mode == nullptr) {
@ -332,20 +343,20 @@ void Plane::check_long_failsafe()
} }
if (failsafe.rc_failsafe && if (failsafe.rc_failsafe &&
(tnow - radio_timeout_ms) > g.fs_timeout_long*1000) { (tnow - radio_timeout_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE); failsafe_long_on_event(FAILSAFE_LONG, ModeReason::RADIO_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == &mode_auto && } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == &mode_auto &&
failsafe.last_heartbeat_ms != 0 && failsafe.last_heartbeat_ms != 0 &&
(tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) { (tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT && } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT &&
failsafe.last_heartbeat_ms != 0 && failsafe.last_heartbeat_ms != 0 &&
(tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) { (tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
gcs().chan(0) != nullptr && gcs().chan(0) != nullptr &&
gcs().chan(0)->last_radio_status_remrssi_ms != 0 && gcs().chan(0)->last_radio_status_remrssi_ms != 0 &&
(tnow - gcs().chan(0)->last_radio_status_remrssi_ms) > g.fs_timeout_long*1000) { (tnow - gcs().chan(0)->last_radio_status_remrssi_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE);
} }
} else { } else {
uint32_t timeout_seconds = g.fs_timeout_long; uint32_t timeout_seconds = g.fs_timeout_long;
@ -356,10 +367,10 @@ void Plane::check_long_failsafe()
// We do not change state but allow for user to change mode // We do not change state but allow for user to change mode
if (failsafe.state == FAILSAFE_GCS && if (failsafe.state == FAILSAFE_GCS &&
(tnow - failsafe.last_heartbeat_ms) < timeout_seconds*1000) { (tnow - failsafe.last_heartbeat_ms) < timeout_seconds*1000) {
failsafe_long_off_event(MODE_REASON_GCS_FAILSAFE); failsafe_long_off_event(ModeReason::GCS_FAILSAFE);
} else if (failsafe.state == FAILSAFE_LONG && } else if (failsafe.state == FAILSAFE_LONG &&
!failsafe.rc_failsafe) { !failsafe.rc_failsafe) {
failsafe_long_off_event(MODE_REASON_RADIO_FAILSAFE); failsafe_long_off_event(ModeReason::RADIO_FAILSAFE);
} }
} }
} }
@ -373,13 +384,13 @@ void Plane::check_short_failsafe()
flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) { flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
// The condition is checked and the flag rc_failsafe is set in radio.cpp // The condition is checked and the flag rc_failsafe is set in radio.cpp
if(failsafe.rc_failsafe) { if(failsafe.rc_failsafe) {
failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE); failsafe_short_on_event(FAILSAFE_SHORT, ModeReason::RADIO_FAILSAFE);
} }
} }
if(failsafe.state == FAILSAFE_SHORT) { if(failsafe.state == FAILSAFE_SHORT) {
if(!failsafe.rc_failsafe || g.fs_action_short == FS_ACTION_SHORT_DISABLED) { if(!failsafe.rc_failsafe || g.fs_action_short == FS_ACTION_SHORT_DISABLED) {
failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE); failsafe_short_off_event(ModeReason::RADIO_FAILSAFE);
} }
} }
} }

Write Preview
Loading…
Cancel
Save