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

Plane: add reason to set_mode()

Browse Source
This commit is contained in:
Tom Pittenger 2016-08-13 00:54:37 -07:00
parent 285e41bac6
commit b187e1f9a6
10 changed files with 68 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -510,7 +510,7 @@ void Plane::handle_auto_mode(void)
if (mission.state() != AP_Mission::MISSION_RUNNING) {
// this should never be reached
set_mode(RTL);
set_mode(RTL, MODE_REASON_MISSION_END);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
@ -779,7 +779,7 @@ void Plane::update_navigation()
case RTL:
if (quadplane.available() && quadplane.rtl_mode == 1 &&
nav_controller->reached_loiter_target()) {
set_mode(QRTL);
set_mode(QRTL, MODE_REASON_UNKNOWN);
break;
} else if (g.rtl_autoland == 1 &&
!auto_state.checked_for_autoland &&

14
ArduPlane/GCS_Mavlink.cpp
View File

@ -1199,7 +1199,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
// location is valid load and set
if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
(plane.control_mode == GUIDED)) {
plane.set_mode(GUIDED);
plane.set_mode(GUIDED, MODE_REASON_GCS_COMMAND);
plane.guided_WP_loc = requested_position;
// add home alt if needed
@ -1267,12 +1267,12 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_NAV_LOITER_UNLIM:
plane.set_mode(LOITER);
plane.set_mode(LOITER, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
plane.set_mode(RTL);
plane.set_mode(RTL, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
@ -1334,7 +1334,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_MISSION_START:
plane.set_mode(AUTO);
plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
@ -1476,19 +1476,19 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
plane.set_mode(MANUAL);
plane.set_mode(MANUAL, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
plane.set_mode(AUTO);
plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_STABILIZE_DISARMED:
case MAV_MODE_STABILIZE_ARMED:
plane.set_mode(FLY_BY_WIRE_A);
plane.set_mode(FLY_BY_WIRE_A, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;

10
ArduPlane/Plane.h
View File

@ -317,7 +317,9 @@ private:
// This is the state of the flight control system
// There are multiple states defined such as MANUAL, FBW-A, AUTO
enum FlightMode control_mode = INITIALISING;
mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
enum FlightMode previous_mode = INITIALISING;
mode_reason_t previous_mode_reason = MODE_REASON_UNKNOWN;
// Used to maintain the state of the previous control switch position
// This is set to 254 when we need to re-read the switch
@ -915,9 +917,9 @@ private:
void autotune_restore(void);
void autotune_enable(bool enable);
bool fly_inverted(void);
void failsafe_short_on_event(enum failsafe_state fstype);
void failsafe_long_on_event(enum failsafe_state fstype);
void failsafe_short_off_event();
void failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason);
void failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason);
void failsafe_short_off_event(mode_reason_t reason);
void low_battery_event(void);
void update_events(void);
uint8_t max_fencepoints(void);
@ -989,7 +991,7 @@ private:
void init_ardupilot();
void startup_ground(void);
enum FlightMode get_previous_mode();
void set_mode(enum FlightMode mode);
void set_mode(enum FlightMode mode, mode_reason_t reason);
bool mavlink_set_mode(uint8_t mode);
void exit_mode(enum FlightMode mode);
void check_long_failsafe();

4
ArduPlane/commands_logic.cpp
View File

@ -81,7 +81,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
set_mode(RTL);
set_mode(RTL, MODE_REASON_UNKNOWN);
break;
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
@ -989,7 +989,7 @@ bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
void Plane::exit_mission_callback()
{
if (control_mode == AUTO) {
set_mode(RTL);
set_mode(RTL, MODE_REASON_MISSION_END);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL");
}
}

2
ArduPlane/control_modes.cpp
View File

@ -41,7 +41,7 @@ void Plane::read_control_switch()
return;
}
set_mode((enum FlightMode)(flight_modes[switchPosition].get()));
set_mode((enum FlightMode)(flight_modes[switchPosition].get()), MODE_REASON_TX_COMMAND);
oldSwitchPosition = switchPosition;
}

15
ArduPlane/defines.h
View File

@ -68,6 +68,21 @@ enum FlightMode {
QRTL = 21
};
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,
};
// type of stick mixing enabled
enum StickMixing {
STICK_MIXING_DISABLED = 0,

30
ArduPlane/events.cpp
View File

@ -2,7 +2,7 @@
#include "Plane.h"
void Plane::failsafe_short_on_event(enum failsafe_state fstype)
void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason)
{
// This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype;
@ -21,9 +21,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
if(g.short_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
set_mode(FLY_BY_WIRE_A, reason);
} else {
set_mode(CIRCLE);
set_mode(CIRCLE, reason);
}
break;
@ -32,7 +32,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
case QHOVER:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
set_mode(QLAND);
set_mode(QLAND, reason);
break;
case AUTO:
@ -43,9 +43,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
if(g.short_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
set_mode(FLY_BY_WIRE_A, reason);
} else {
set_mode(CIRCLE);
set_mode(CIRCLE, reason);
}
}
break;
@ -60,7 +60,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
}
void Plane::failsafe_long_on_event(enum failsafe_state fstype)
void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason)
{
// This is how to handle a long loss of control signal failsafe.
gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on, ");
@ -83,16 +83,16 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
parachute_release();
#endif
} else if (g.long_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
set_mode(FLY_BY_WIRE_A, reason);
} else {
set_mode(RTL);
set_mode(RTL, reason);
}
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
set_mode(QLAND);
set_mode(QLAND, reason);
break;
case AUTO:
@ -104,9 +104,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
parachute_release();
#endif
} else if (g.long_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
set_mode(FLY_BY_WIRE_A, reason);
} else if (g.long_fs_action == 1) {
set_mode(RTL);
set_mode(RTL, reason);
}
break;
@ -122,7 +122,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
}
void Plane::failsafe_short_off_event()
void Plane::failsafe_short_off_event(mode_reason_t reason)
{
// We're back in radio contact
gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off");
@ -132,7 +132,7 @@ void Plane::failsafe_short_off_event()
// --------------------------------------------------------
if (control_mode == CIRCLE && failsafe.saved_mode_set) {
failsafe.saved_mode_set = 0;
set_mode(failsafe.saved_mode);
set_mode(failsafe.saved_mode, reason);
}
}
@ -146,7 +146,7 @@ void Plane::low_battery_event(void)
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
set_mode(RTL);
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}
failsafe.low_battery = true;

8
ArduPlane/geofence.cpp
View File

@ -292,7 +292,7 @@ void Plane::geofence_check(bool altitude_check_only)
guided_WP_loc.lat == geofence_state->guided_lat &&
guided_WP_loc.lng == geofence_state->guided_lng) {
geofence_state->old_switch_position = 254;
set_mode(get_previous_mode());
set_mode(get_previous_mode(), MODE_REASON_GCS_COMMAND);
}
return;
}
@ -311,7 +311,7 @@ void Plane::geofence_check(bool altitude_check_only)
struct Location loc;
// Never trigger a fence breach in the final stage of landing
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) {
return;
}
@ -380,9 +380,9 @@ void Plane::geofence_check(bool altitude_check_only)
int8_t saved_auto_trim = g.auto_trim;
g.auto_trim.set(0);
if (g.fence_action == FENCE_ACTION_RTL) {
set_mode(RTL);
set_mode(RTL, MODE_REASON_FENCE_BREACH);
} else {
set_mode(GUIDED);
set_mode(GUIDED, MODE_REASON_FENCE_BREACH);
}
g.auto_trim.set(saved_auto_trim);

6
ArduPlane/landing.cpp
View File

@ -33,7 +33,7 @@ bool Plane::verify_land()
mission.resume();
if (!success) {
// on a restart failure lets RTL or else the plane may fly away with nowhere to go!
set_mode(RTL);
set_mode(RTL, MODE_REASON_MISSION_END);
}
// make sure to return false so it leaves the mission index alone
}
@ -368,7 +368,9 @@ bool Plane::jump_to_landing_sequence(void)
uint16_t land_idx = mission.get_landing_sequence_start();
if (land_idx != 0) {
if (mission.set_current_cmd(land_idx)) {
set_mode(AUTO);
// in case we're in RTL
set_mode(AUTO, MODE_REASON_UNKNOWN);
//if the mission has ended it has to be restarted
if (mission.state() == AP_Mission::MISSION_STOPPED) {

22
ArduPlane/system.cpp
View File

@ -245,7 +245,7 @@ void Plane::init_ardupilot()
// choose the nav controller
set_nav_controller();
set_mode((FlightMode)g.initial_mode.get());
set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN);
// set the correct flight mode
// ---------------------------
@ -265,7 +265,7 @@ void Plane::init_ardupilot()
//********************************************************************************
void Plane::startup_ground(void)
{
set_mode(INITIALISING);
set_mode(INITIALISING, MODE_REASON_UNKNOWN);
gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> Ground start");
@ -311,7 +311,7 @@ enum FlightMode Plane::get_previous_mode() {
return previous_mode;
}
void Plane::set_mode(enum FlightMode mode)
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
@ -352,6 +352,8 @@ void Plane::set_mode(enum FlightMode mode)
// set mode
previous_mode = control_mode;
control_mode = mode;
previous_mode_reason = control_mode_reason;
control_mode_reason = reason;
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
@ -518,7 +520,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
case QLOITER:
case QLAND:
case QRTL:
set_mode((enum FlightMode)mode);
set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND);
return true;
}
return false;
@ -552,19 +554,19 @@ void Plane::check_long_failsafe()
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
if (failsafe.state == FAILSAFE_SHORT &&
(tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_LONG);
failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO &&
failsafe.last_heartbeat_ms != 0 &&
(tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS);
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT &&
failsafe.last_heartbeat_ms != 0 &&
(tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS);
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
gcs[0].last_radio_status_remrssi_ms != 0 &&
(tnow - gcs[0].last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS);
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
}
} else {
// We do not change state but allow for user to change mode
@ -588,13 +590,13 @@ void Plane::check_short_failsafe()
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
// The condition is checked and the flag ch3_failsafe is set in radio.cpp
if(failsafe.ch3_failsafe) {
failsafe_short_on_event(FAILSAFE_SHORT);
failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE);
}
}
if(failsafe.state == FAILSAFE_SHORT) {
if(!failsafe.ch3_failsafe) {
failsafe_short_off_event();
failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE);
}
}
}