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

Rover: Support new AP_Vehicle::set_mode

Browse Source
This commit is contained in:
Michael du Breuil 2019-10-16 20:48:47 -07:00 committed by Randy Mackay
parent c369139be0
commit a1acc75e11
12 changed files with 40 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/GCS_Mavlink.cpp
View File

@ -620,13 +620,13 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
switch (packet.command) {
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND)) {
if (rover.set_mode(rover.mode_rtl, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
case MAV_CMD_MISSION_START:
if (rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND)) {
if (rover.set_mode(rover.mode_auto, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
@ -1092,12 +1092,3 @@ void Rover::mavlink_delay_cb()
logger.EnableWrites(true);
}
bool GCS_MAVLINK_Rover::set_mode(const uint8_t mode)
{
Mode *new_mode = rover.mode_from_mode_num((enum Mode::Number)mode);
if (new_mode == nullptr) {
return false;
}
return rover.set_mode(*new_mode, MODE_REASON_GCS_COMMAND);
}

2
APMrover2/GCS_Mavlink.h
View File

@ -15,8 +15,6 @@ protected:
uint8_t sysid_my_gcs() 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_int_packet(const mavlink_command_int_t &packet) override;
MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;

4
APMrover2/RC_Channel.cpp
View File

@ -23,7 +23,7 @@ void RC_Channel_Rover::mode_switch_changed(modeswitch_pos_t new_pos)
}
Mode *new_mode = rover.mode_from_mode_num((Mode::Number)rover.modes[new_pos].get());
if (new_mode != nullptr) {
rover.set_mode(*new_mode, MODE_REASON_TX_COMMAND);
rover.set_mode(*new_mode, ModeReason::RC_COMMAND);
}
}
@ -71,7 +71,7 @@ void RC_Channel_Rover::do_aux_function_change_mode(Mode &mode,
{
switch (ch_flag) {
case HIGH:
rover.set_mode(mode, MODE_REASON_TX_COMMAND);
rover.set_mode(mode, ModeReason::RC_COMMAND);
break;
case MIDDLE:
// do nothing

5
APMrover2/Rover.h
View File

@ -214,7 +214,7 @@ private:
// This is the state of the flight control system
// There are multiple states defined such as MANUAL, AUTO, ...
Mode *control_mode;
mode_reason_t control_mode_reason = MODE_REASON_INITIALISED;
ModeReason control_mode_reason = ModeReason::UNKNOWN;
// Used to maintain the state of the previous control switch position
// This is set to -1 when we need to re-read the switch
@ -401,7 +401,8 @@ private:
void init_ardupilot();
void startup_ground(void);
void update_ahrs_flyforward();
bool set_mode(Mode &new_mode, mode_reason_t reason);
bool set_mode(Mode &new_mode, ModeReason reason);
bool set_mode(const uint8_t new_mode, ModeReason reason) override;
bool mavlink_set_mode(uint8_t mode);
void startup_INS_ground(void);
void notify_mode(const Mode *new_mode);

2
APMrover2/crash_check.cpp
View File

@ -56,7 +56,7 @@ void Rover::crash_check()
// send message to gcs
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
// change mode to hold and disarm
set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE);
set_mode(mode_hold, ModeReason::CRASH_FAILSAFE);
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
arming.disarm();
}

12
APMrover2/defines.h
View File

@ -90,18 +90,6 @@ enum fs_ekf_action {
#define DISTANCE_HOME_MINCHANGE 0.5f // minimum distance to adjust home location
enum mode_reason_t {
MODE_REASON_INITIALISED = 0,
MODE_REASON_TX_COMMAND,
MODE_REASON_GCS_COMMAND,
MODE_REASON_FAILSAFE,
MODE_REASON_MISSION_END,
MODE_REASON_CRASH_FAILSAFE,
MODE_REASON_MISSION_COMMAND,
MODE_REASON_FENCE_BREACH,
MODE_REASON_EKF_FAILSAFE,
};
enum pilot_steer_type_t {
PILOT_STEER_TYPE_DEFAULT = 0,
PILOT_STEER_TYPE_TWO_PADDLES = 1,

2
APMrover2/ekf_check.cpp
View File

@ -167,7 +167,7 @@ void Rover::failsafe_ekf_event()
break;
case FS_EFK_HOLD:
default:
set_mode(mode_hold, MODE_REASON_EKF_FAILSAFE);
set_mode(mode_hold, ModeReason::EKF_FAILSAFE);
break;
}
}

26
APMrover2/failsafe.cpp
View File

@ -84,23 +84,23 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
case Failsafe_Action_None:
break;
case Failsafe_Action_RTL:
if (!set_mode(mode_rtl, MODE_REASON_FAILSAFE)) {
set_mode(mode_hold, MODE_REASON_FAILSAFE);
if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
}
break;
case Failsafe_Action_Hold:
set_mode(mode_hold, MODE_REASON_FAILSAFE);
set_mode(mode_hold, ModeReason::FAILSAFE);
break;
case Failsafe_Action_SmartRTL:
if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
if (!set_mode(mode_rtl, MODE_REASON_FAILSAFE)) {
set_mode(mode_hold, MODE_REASON_FAILSAFE);
if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
}
}
break;
case Failsafe_Action_SmartRTL_Hold:
if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
set_mode(mode_hold, MODE_REASON_FAILSAFE);
if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
}
break;
}
@ -114,21 +114,21 @@ void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
case Failsafe_Action_None:
break;
case Failsafe_Action_SmartRTL:
if (set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
break;
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (set_mode(mode_rtl, MODE_REASON_FAILSAFE)) {
if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {
break;
}
FALLTHROUGH;
case Failsafe_Action_Hold:
set_mode(mode_hold, MODE_REASON_FAILSAFE);
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Hold:
if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
set_mode(mode_hold, MODE_REASON_FAILSAFE);
if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
}
break;
case Failsafe_Action_Terminate:

6
APMrover2/fence.cpp
View File

@ -20,12 +20,12 @@ void Rover::fence_check()
if (g2.fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
// if we are within 100m of the fence, RTL
if (g2.fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(mode_rtl, MODE_REASON_FENCE_BREACH)) {
set_mode(mode_hold, MODE_REASON_FENCE_BREACH);
if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) {
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
}
} else {
// if more than 100m outside the fence just force to HOLD
set_mode(mode_hold, MODE_REASON_FENCE_BREACH);
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
}
}
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));

2
APMrover2/mode.h
View File

@ -18,7 +18,7 @@ public:
// Auto Pilot modes
// ----------------
enum Number {
enum Number : uint8_t {
MANUAL = 0,
ACRO = 1,
STEERING = 3,

2
APMrover2/mode_auto.cpp
View File

@ -441,7 +441,7 @@ void ModeAuto::exit_mission()
return;
}
if (g2.mis_done_behave == MIS_DONE_BEHAVE_ACRO && rover.set_mode(rover.mode_acro, MODE_REASON_MISSION_END)) {
if (g2.mis_done_behave == MIS_DONE_BEHAVE_ACRO && rover.set_mode(rover.mode_acro, ModeReason::MISSION_END)) {
return;
}

16
APMrover2/system.cpp
View File

@ -150,7 +150,7 @@ void Rover::init_ardupilot()
if (initial_mode == nullptr) {
initial_mode = &mode_initializing;
}
set_mode(*initial_mode, MODE_REASON_INITIALISED);
set_mode(*initial_mode, ModeReason::INITIALISED);
// initialise rc channels
rc().init();
@ -173,7 +173,7 @@ void Rover::init_ardupilot()
//*********************************************************************************
void Rover::startup_ground(void)
{
set_mode(mode_initializing, MODE_REASON_INITIALISED);
set_mode(mode_initializing, ModeReason::INITIALISED);
gcs().send_text(MAV_SEVERITY_INFO, "<startup_ground> Ground start");
@ -240,7 +240,7 @@ void Rover::update_ahrs_flyforward()
ahrs.set_fly_forward(flyforward);
}
bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
bool Rover::set_mode(Mode &new_mode, ModeReason reason)
{
if (control_mode == &new_mode) {
// don't switch modes if we are already in the correct mode.
@ -277,6 +277,16 @@ bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
return true;
}
bool Rover::set_mode(const uint8_t new_mode, ModeReason reason)
{
static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
Mode *mode = rover.mode_from_mode_num((enum Mode::Number)new_mode);
if (mode == nullptr) {
return false;
}
return rover.set_mode(*mode, reason);
}
void Rover::startup_INS_ground(void)
{
gcs().send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle");