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

Sub: Support new AP_Vehicle::set_mode

This commit is contained in:
Michael du Breuil 2019-10-16 20:50:07 -07:00 committed by Andrew Tridgell
parent e5a1654bb6
commit b0f66237de
9 changed files with 34 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduSub/GCS_Mavlink.cpp
View File

@ -458,13 +458,13 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
{
switch (packet.command) {
case MAV_CMD_NAV_LOITER_UNLIM:
if (!sub.set_mode(POSHOLD, MODE_REASON_GCS_COMMAND)) {
if (!sub.set_mode(POSHOLD, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_LAND:
if (!sub.set_mode(SURFACE, MODE_REASON_GCS_COMMAND)) {
if (!sub.set_mode(SURFACE, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
@ -494,7 +494,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
return MAV_RESULT_FAILED;
case MAV_CMD_MISSION_START:
if (sub.motors.armed() && sub.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
if (sub.motors.armed() && sub.set_mode(AUTO, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
@ -825,11 +825,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long
return MAV_RESULT_FAILED;
}
bool GCS_MAVLINK_Sub::set_mode(uint8_t mode)
{
return sub.set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND);
}
int32_t GCS_MAVLINK_Sub::global_position_int_alt() const {
if (!sub.ap.depth_sensor_present) {
return 0;

1
ArduSub/GCS_Mavlink.h
View File

@ -18,7 +18,6 @@ protected:
uint8_t sysid_my_gcs() const override;
bool set_mode(uint8_t mode) override;
bool should_zero_rc_outputs_on_reboot() const override { return true; }
MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;

7
ArduSub/Sub.h
View File

@ -230,10 +230,10 @@ private:
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
control_mode_t control_mode;
mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
ModeReason control_mode_reason = ModeReason::UNKNOWN;
control_mode_t prev_control_mode;
mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN;
ModeReason prev_control_mode_reason = ModeReason::UNKNOWN;
#if RCMAP_ENABLED == ENABLED
RCMapper rcmap;
@ -549,7 +549,8 @@ private:
void mainloop_failsafe_enable();
void mainloop_failsafe_disable();
void fence_check();
bool set_mode(control_mode_t mode, mode_reason_t reason);
bool set_mode(control_mode_t mode, ModeReason reason);
bool set_mode(const uint8_t mode, const ModeReason reason) override;
void update_flight_mode();
void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode);
bool mode_requires_GPS(control_mode_t mode);

2
ArduSub/commands_logic.cpp
View File

@ -216,7 +216,7 @@ void Sub::exit_mission()
// Try to enter loiter, if that fails, go to depth hold
if (!auto_loiter_start()) {
set_mode(ALT_HOLD, MODE_REASON_MISSION_END);
set_mode(ALT_HOLD, ModeReason::MISSION_END);
}
}

2
ArduSub/control_surface.cpp
View File

@ -35,7 +35,7 @@ void Sub::surface_run()
// Already at surface, hold depth at surface
if (ap.at_surface) {
set_mode(ALT_HOLD, MODE_REASON_SURFACE_COMPLETE);
set_mode(ALT_HOLD, ModeReason::SURFACE_COMPLETE);
}
// convert pilot input to lean angles

20
ArduSub/defines.h
View File

@ -30,7 +30,7 @@ enum autopilot_yaw_mode {
};
// Auto Pilot Modes enumeration
enum control_mode_t {
enum control_mode_t : uint8_t {
STABILIZE = 0, // manual angle with manual depth/throttle
ACRO = 1, // manual body-frame angular rate with manual depth/throttle
ALT_HOLD = 2, // manual angle with automatic depth/throttle
@ -43,24 +43,6 @@ enum control_mode_t {
MOTOR_DETECT = 20 // Automatically detect motors orientation
};
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_SURFACE_ESCAPE,
MODE_REASON_FENCE_BREACH,
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_SURFACE_COMPLETE,
MODE_REASON_LEAK_FAILSAFE,
MODE_REASON_BAD_DEPTH
};
// Acro Trainer types
#define ACRO_TRAINER_DISABLED 0
#define ACRO_TRAINER_LEVELING 1

16
ArduSub/failsafe.cpp
View File

@ -90,7 +90,7 @@ void Sub::failsafe_sensors_check()
if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
// This should always succeed
if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
if (!set_mode(MANUAL, ModeReason::BAD_DEPTH)) {
// We should never get here
arming.disarm();
}
@ -157,7 +157,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
switch((Failsafe_Action)action) {
case Failsafe_Action_Surface:
set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
set_mode(SURFACE, ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_Disarm:
arming.disarm();
@ -300,7 +300,7 @@ void Sub::failsafe_leak_check()
// Handle failsafe action
if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) {
set_mode(SURFACE, MODE_REASON_LEAK_FAILSAFE);
set_mode(SURFACE, ModeReason::LEAK_FAILSAFE);
}
}
@ -347,11 +347,11 @@ void Sub::failsafe_gcs_check()
if (g.failsafe_gcs == FS_GCS_DISARM) {
arming.disarm();
} else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
if (!set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE)) {
if (!set_mode(ALT_HOLD, ModeReason::GCS_FAILSAFE)) {
arming.disarm();
}
} else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
if (!set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE)) {
if (!set_mode(SURFACE, ModeReason::GCS_FAILSAFE)) {
arming.disarm();
}
}
@ -477,14 +477,14 @@ void Sub::failsafe_terrain_act()
{
switch (g.failsafe_terrain) {
case FS_TERRAIN_HOLD:
if (!set_mode(POSHOLD, MODE_REASON_TERRAIN_FAILSAFE)) {
set_mode(ALT_HOLD, MODE_REASON_TERRAIN_FAILSAFE);
if (!set_mode(POSHOLD, ModeReason::TERRAIN_FAILSAFE)) {
set_mode(ALT_HOLD, ModeReason::TERRAIN_FAILSAFE);
}
AP_Notify::events.failsafe_mode_change = 1;
break;
case FS_TERRAIN_SURFACE:
set_mode(SURFACE, MODE_REASON_TERRAIN_FAILSAFE);
set_mode(SURFACE, ModeReason::TERRAIN_FAILSAFE);
AP_Notify::events.failsafe_mode_change = 1;
break;

8
ArduSub/flight_mode.cpp
View File

@ -2,7 +2,7 @@
// change flight mode and perform any necessary initialisation
// returns true if mode was successfully set
bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
bool Sub::set_mode(control_mode_t mode, ModeReason reason)
{
// boolean to record if flight mode could be set
bool success = false;
@ -99,6 +99,12 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
return success;
}
bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason)
{
static_assert(sizeof(control_mode_t) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
return sub.set_mode((control_mode_t)new_mode, reason);
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void Sub::update_flight_mode()

18
ArduSub/joystick.cpp
View File

@ -35,7 +35,7 @@ void Sub::init_joystick()
lights1 = RC_Channels::rc_channel(8)->get_radio_min();
lights2 = RC_Channels::rc_channel(9)->get_radio_min();
set_mode(MANUAL, MODE_REASON_TX_COMMAND); // Initialize flight mode
set_mode(MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode
if (g.numGainSettings < 1) {
g.numGainSettings.set_and_save(1);
@ -158,28 +158,28 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
break;
case JSButton::button_function_t::k_mode_manual:
set_mode(MANUAL, MODE_REASON_TX_COMMAND);
set_mode(MANUAL, ModeReason::RC_COMMAND);
break;
case JSButton::button_function_t::k_mode_stabilize:
set_mode(STABILIZE, MODE_REASON_TX_COMMAND);
set_mode(STABILIZE, ModeReason::RC_COMMAND);
break;
case JSButton::button_function_t::k_mode_depth_hold:
set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND);
set_mode(ALT_HOLD, ModeReason::RC_COMMAND);
break;
case JSButton::button_function_t::k_mode_auto:
set_mode(AUTO, MODE_REASON_TX_COMMAND);
set_mode(AUTO, ModeReason::RC_COMMAND);
break;
case JSButton::button_function_t::k_mode_guided:
set_mode(GUIDED, MODE_REASON_TX_COMMAND);
set_mode(GUIDED, ModeReason::RC_COMMAND);
break;
case JSButton::button_function_t::k_mode_circle:
set_mode(CIRCLE, MODE_REASON_TX_COMMAND);
set_mode(CIRCLE, ModeReason::RC_COMMAND);
break;
case JSButton::button_function_t::k_mode_acro:
set_mode(ACRO, MODE_REASON_TX_COMMAND);
set_mode(ACRO, ModeReason::RC_COMMAND);
break;
case JSButton::button_function_t::k_mode_poshold:
set_mode(POSHOLD, MODE_REASON_TX_COMMAND);
set_mode(POSHOLD, ModeReason::RC_COMMAND);
break;
case JSButton::button_function_t::k_mount_center: