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

Rover: Support new AP_Vehicle::set_mode

master
Michael du Breuil 5 years ago committed by Randy Mackay
parent
c369139be0
commit
a1acc75e11
12 changed files with 40 additions and 52 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. 13
      APMrover2/GCS_Mavlink.cpp
  2. 2
      APMrover2/GCS_Mavlink.h
  3. 4
      APMrover2/RC_Channel.cpp
  4. 5
      APMrover2/Rover.h
  5. 2
      APMrover2/crash_check.cpp
  6. 12
      APMrover2/defines.h
  7. 2
      APMrover2/ekf_check.cpp
  8. 26
      APMrover2/failsafe.cpp
  9. 6
      APMrover2/fence.cpp
  10. 2
      APMrover2/mode.h
  11. 2
      APMrover2/mode_auto.cpp
  12. 16
      APMrover2/system.cpp

13
APMrover2/GCS_Mavlink.cpp
Unescape View File

@ -620,13 +620,13 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
switch (packet.command) { switch (packet.command) {
case MAV_CMD_NAV_RETURN_TO_LAUNCH: 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_ACCEPTED;
} }
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
case MAV_CMD_MISSION_START: 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_ACCEPTED;
} }
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
@ -1092,12 +1092,3 @@ void Rover::mavlink_delay_cb()
logger.EnableWrites(true); 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
Unescape View File

@ -15,8 +15,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_int_packet(const mavlink_command_int_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; MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;

4
APMrover2/RC_Channel.cpp
Unescape 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()); Mode *new_mode = rover.mode_from_mode_num((Mode::Number)rover.modes[new_pos].get());
if (new_mode != nullptr) { 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) { switch (ch_flag) {
case HIGH: case HIGH:
rover.set_mode(mode, MODE_REASON_TX_COMMAND); rover.set_mode(mode, ModeReason::RC_COMMAND);
break; break;
case MIDDLE: case MIDDLE:
// do nothing // do nothing

5
APMrover2/Rover.h
Unescape View File

@ -214,7 +214,7 @@ 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, AUTO, ... // There are multiple states defined such as MANUAL, AUTO, ...
Mode *control_mode; 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 // Used to maintain the state of the previous control switch position
// This is set to -1 when we need to re-read the switch // This is set to -1 when we need to re-read the switch
@ -401,7 +401,8 @@ private:
void init_ardupilot(); void init_ardupilot();
void startup_ground(void); void startup_ground(void);
void update_ahrs_flyforward(); 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); bool mavlink_set_mode(uint8_t mode);
void startup_INS_ground(void); void startup_INS_ground(void);
void notify_mode(const Mode *new_mode); void notify_mode(const Mode *new_mode);

2
APMrover2/crash_check.cpp
Unescape View File

@ -56,7 +56,7 @@ void Rover::crash_check()
// send message to gcs // send message to gcs
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
// change mode to hold and disarm // 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) { if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
arming.disarm(); arming.disarm();
} }

12
APMrover2/defines.h
Unescape View File

@ -90,18 +90,6 @@ enum fs_ekf_action {
#define DISTANCE_HOME_MINCHANGE 0.5f // minimum distance to adjust home location #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 { enum pilot_steer_type_t {
PILOT_STEER_TYPE_DEFAULT = 0, PILOT_STEER_TYPE_DEFAULT = 0,
PILOT_STEER_TYPE_TWO_PADDLES = 1, PILOT_STEER_TYPE_TWO_PADDLES = 1,

2
APMrover2/ekf_check.cpp
Unescape View File

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

26
APMrover2/failsafe.cpp
Unescape View File

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

6
APMrover2/fence.cpp
Unescape View File

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

2
APMrover2/mode.h
Unescape View File

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

2
APMrover2/mode_auto.cpp
Unescape View File

@ -441,7 +441,7 @@ void ModeAuto::exit_mission()
return; 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; return;
} }

16
APMrover2/system.cpp
Unescape View File

@ -150,7 +150,7 @@ void Rover::init_ardupilot()
if (initial_mode == nullptr) { if (initial_mode == nullptr) {
initial_mode = &mode_initializing; initial_mode = &mode_initializing;
} }
set_mode(*initial_mode, MODE_REASON_INITIALISED); set_mode(*initial_mode, ModeReason::INITIALISED);
// initialise rc channels // initialise rc channels
rc().init(); rc().init();
@ -173,7 +173,7 @@ void Rover::init_ardupilot()
//********************************************************************************* //*********************************************************************************
void Rover::startup_ground(void) 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"); gcs().send_text(MAV_SEVERITY_INFO, "<startup_ground> Ground start");
@ -240,7 +240,7 @@ void Rover::update_ahrs_flyforward()
ahrs.set_fly_forward(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) { 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.
@ -277,6 +277,16 @@ bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
return true; 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) void Rover::startup_INS_ground(void)
{ {
gcs().send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle"); gcs().send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle");

Write Preview
Loading…
Cancel
Save