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

Rover: make Auto submode enum class

This commit is contained in:
Pierre Kancir 2023-09-21 15:56:25 +02:00 committed by Peter Barker
parent f088f0df9e
commit c7f3d0046d
2 changed files with 95 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Rover/mode.h
View File

@ -251,7 +251,7 @@ public:
bool is_autopilot_mode() const override { return true; } bool is_autopilot_mode() const override { return true; }
// return if external control is allowed in this mode (Guided or Guided-within-Auto) // return if external control is allowed in this mode (Guided or Guided-within-Auto)
bool in_guided_mode() const override { return _submode == Auto_Guided || _submode == Auto_NavScriptTime; } bool in_guided_mode() const override { return _submode == SubMode::Guided || _submode == SubMode::NavScriptTime; }
// return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing() const override; float wp_bearing() const override;
@ -294,15 +294,15 @@ protected:
bool _enter() override; bool _enter() override;
void _exit() override; void _exit() override;
enum AutoSubMode { enum SubMode: uint8_t {
Auto_WP, // drive to a given location WP, // drive to a given location
Auto_HeadingAndSpeed, // turn to a given heading HeadingAndSpeed, // turn to a given heading
Auto_RTL, // perform RTL within auto mode RTL, // perform RTL within auto mode
Auto_Loiter, // perform Loiter within auto mode Loiter, // perform Loiter within auto mode
Auto_Guided, // handover control to external navigation system from within auto mode Guided, // handover control to external navigation system from within auto mode
Auto_Stop, // stop the vehicle as quickly as possible Stop, // stop the vehicle as quickly as possible
Auto_NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing
Auto_Circle, // circle a given location Circle, // circle a given location
} _submode; } _submode;
private: private:

170
Rover/mode_auto.cpp
View File

@ -66,7 +66,7 @@ void ModeAuto::update()
// check for mission changes // check for mission changes
if (mis_change_detector.check_for_mission_change()) { if (mis_change_detector.check_for_mission_change()) {
// if mission is running restart the current command if it is a waypoint command // if mission is running restart the current command if it is a waypoint command
if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == AutoSubMode::Auto_WP)) { if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == SubMode::WP)) {
if (mission.restart_current_nav_cmd()) { if (mission.restart_current_nav_cmd()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
} else { } else {
@ -80,7 +80,7 @@ void ModeAuto::update()
} }
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
{ {
// check if we've reached the destination // check if we've reached the destination
if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) { if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) {
@ -101,7 +101,7 @@ void ModeAuto::update()
break; break;
} }
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
{ {
if (!_reached_heading) { if (!_reached_heading) {
// run steering and throttle controllers // run steering and throttle controllers
@ -122,15 +122,15 @@ void ModeAuto::update()
break; break;
} }
case Auto_RTL: case SubMode::RTL:
rover.mode_rtl.update(); rover.mode_rtl.update();
break; break;
case Auto_Loiter: case SubMode::Loiter:
rover.mode_loiter.update(); rover.mode_loiter.update();
break; break;
case Auto_Guided: case SubMode::Guided:
{ {
// send location target to offboard navigation system // send location target to offboard navigation system
send_guided_position_target(); send_guided_position_target();
@ -138,15 +138,15 @@ void ModeAuto::update()
break; break;
} }
case Auto_Stop: case SubMode::Stop:
stop_vehicle(); stop_vehicle();
break; break;
case Auto_NavScriptTime: case SubMode::NavScriptTime:
rover.mode_guided.update(); rover.mode_guided.update();
break; break;
case Auto_Circle: case SubMode::Circle:
rover.g2.mode_circle.update(); rover.g2.mode_circle.update();
break; break;
} }
@ -166,19 +166,19 @@ void ModeAuto::calc_throttle(float target_speed, bool avoidance_enabled)
float ModeAuto::wp_bearing() const float ModeAuto::wp_bearing() const
{ {
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
return g2.wp_nav.wp_bearing_cd() * 0.01f; return g2.wp_nav.wp_bearing_cd() * 0.01f;
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Auto_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
case Auto_RTL: case SubMode::RTL:
return rover.mode_rtl.wp_bearing(); return rover.mode_rtl.wp_bearing();
case Auto_Loiter: case SubMode::Loiter:
return rover.mode_loiter.wp_bearing(); return rover.mode_loiter.wp_bearing();
case Auto_Guided: case SubMode::Guided:
case Auto_NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.wp_bearing(); return rover.mode_guided.wp_bearing();
case Auto_Circle: case SubMode::Circle:
return rover.g2.mode_circle.wp_bearing(); return rover.g2.mode_circle.wp_bearing();
} }
@ -190,19 +190,19 @@ float ModeAuto::wp_bearing() const
float ModeAuto::nav_bearing() const float ModeAuto::nav_bearing() const
{ {
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
return g2.wp_nav.nav_bearing_cd() * 0.01f; return g2.wp_nav.nav_bearing_cd() * 0.01f;
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Auto_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
case Auto_RTL: case SubMode::RTL:
return rover.mode_rtl.nav_bearing(); return rover.mode_rtl.nav_bearing();
case Auto_Loiter: case SubMode::Loiter:
return rover.mode_loiter.nav_bearing(); return rover.mode_loiter.nav_bearing();
case Auto_Guided: case SubMode::Guided:
case Auto_NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.nav_bearing(); return rover.mode_guided.nav_bearing();
case Auto_Circle: case SubMode::Circle:
return rover.g2.mode_circle.nav_bearing(); return rover.g2.mode_circle.nav_bearing();
} }
@ -214,19 +214,19 @@ float ModeAuto::nav_bearing() const
float ModeAuto::crosstrack_error() const float ModeAuto::crosstrack_error() const
{ {
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
return g2.wp_nav.crosstrack_error(); return g2.wp_nav.crosstrack_error();
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Auto_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
case Auto_RTL: case SubMode::RTL:
return rover.mode_rtl.crosstrack_error(); return rover.mode_rtl.crosstrack_error();
case Auto_Loiter: case SubMode::Loiter:
return rover.mode_loiter.crosstrack_error(); return rover.mode_loiter.crosstrack_error();
case Auto_Guided: case SubMode::Guided:
case Auto_NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.crosstrack_error(); return rover.mode_guided.crosstrack_error();
case Auto_Circle: case SubMode::Circle:
return rover.g2.mode_circle.crosstrack_error(); return rover.g2.mode_circle.crosstrack_error();
} }
@ -238,19 +238,19 @@ float ModeAuto::crosstrack_error() const
float ModeAuto::get_desired_lat_accel() const float ModeAuto::get_desired_lat_accel() const
{ {
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
return g2.wp_nav.get_lat_accel(); return g2.wp_nav.get_lat_accel();
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Auto_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
case Auto_RTL: case SubMode::RTL:
return rover.mode_rtl.get_desired_lat_accel(); return rover.mode_rtl.get_desired_lat_accel();
case Auto_Loiter: case SubMode::Loiter:
return rover.mode_loiter.get_desired_lat_accel(); return rover.mode_loiter.get_desired_lat_accel();
case Auto_Guided: case SubMode::Guided:
case Auto_NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.get_desired_lat_accel(); return rover.mode_guided.get_desired_lat_accel();
case Auto_Circle: case SubMode::Circle:
return rover.g2.mode_circle.get_desired_lat_accel(); return rover.g2.mode_circle.get_desired_lat_accel();
} }
@ -262,20 +262,20 @@ float ModeAuto::get_desired_lat_accel() const
float ModeAuto::get_distance_to_destination() const float ModeAuto::get_distance_to_destination() const
{ {
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
return _distance_to_destination; return _distance_to_destination;
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Auto_Stop: case SubMode::Stop:
// no valid distance so return zero // no valid distance so return zero
return 0.0f; return 0.0f;
case Auto_RTL: case SubMode::RTL:
return rover.mode_rtl.get_distance_to_destination(); return rover.mode_rtl.get_distance_to_destination();
case Auto_Loiter: case SubMode::Loiter:
return rover.mode_loiter.get_distance_to_destination(); return rover.mode_loiter.get_distance_to_destination();
case Auto_Guided: case SubMode::Guided:
case Auto_NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.get_distance_to_destination(); return rover.mode_guided.get_distance_to_destination();
case Auto_Circle: case SubMode::Circle:
return rover.g2.mode_circle.get_distance_to_destination(); return rover.g2.mode_circle.get_distance_to_destination();
} }
@ -287,24 +287,24 @@ float ModeAuto::get_distance_to_destination() const
bool ModeAuto::get_desired_location(Location& destination) const bool ModeAuto::get_desired_location(Location& destination) const
{ {
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
if (g2.wp_nav.is_destination_valid()) { if (g2.wp_nav.is_destination_valid()) {
destination = g2.wp_nav.get_oa_destination(); destination = g2.wp_nav.get_oa_destination();
return true; return true;
} }
return false; return false;
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Auto_Stop: case SubMode::Stop:
// no desired location for this submode // no desired location for this submode
return false; return false;
case Auto_RTL: case SubMode::RTL:
return rover.mode_rtl.get_desired_location(destination); return rover.mode_rtl.get_desired_location(destination);
case Auto_Loiter: case SubMode::Loiter:
return rover.mode_loiter.get_desired_location(destination); return rover.mode_loiter.get_desired_location(destination);
case Auto_Guided: case SubMode::Guided:
case Auto_NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.get_desired_location(destination); return rover.mode_guided.get_desired_location(destination);
case Auto_Circle: case SubMode::Circle:
return rover.g2.mode_circle.get_desired_location(destination); return rover.g2.mode_circle.get_desired_location(destination);
} }
@ -320,7 +320,7 @@ bool ModeAuto::set_desired_location(const Location &destination, Location next_d
return false; return false;
} }
_submode = Auto_WP; _submode = SubMode::WP;
return true; return true;
} }
@ -329,24 +329,24 @@ bool ModeAuto::set_desired_location(const Location &destination, Location next_d
bool ModeAuto::reached_destination() const bool ModeAuto::reached_destination() const
{ {
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
return g2.wp_nav.reached_destination(); return g2.wp_nav.reached_destination();
break; break;
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Auto_Stop: case SubMode::Stop:
// always return true because this is the safer option to allow missions to continue // always return true because this is the safer option to allow missions to continue
return true; return true;
break; break;
case Auto_RTL: case SubMode::RTL:
return rover.mode_rtl.reached_destination(); return rover.mode_rtl.reached_destination();
break; break;
case Auto_Loiter: case SubMode::Loiter:
return rover.mode_loiter.reached_destination(); return rover.mode_loiter.reached_destination();
break; break;
case Auto_Guided: case SubMode::Guided:
case Auto_NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.reached_destination(); return rover.mode_guided.reached_destination();
case Auto_Circle: case SubMode::Circle:
return rover.g2.mode_circle.reached_destination(); return rover.g2.mode_circle.reached_destination();
} }
@ -358,20 +358,20 @@ bool ModeAuto::reached_destination() const
bool ModeAuto::set_desired_speed(float speed) bool ModeAuto::set_desired_speed(float speed)
{ {
switch (_submode) { switch (_submode) {
case Auto_WP: case SubMode::WP:
case Auto_Stop: case SubMode::Stop:
return g2.wp_nav.set_speed_max(speed); return g2.wp_nav.set_speed_max(speed);
case Auto_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
_desired_speed = speed; _desired_speed = speed;
return true; return true;
case Auto_RTL: case SubMode::RTL:
return rover.mode_rtl.set_desired_speed(speed); return rover.mode_rtl.set_desired_speed(speed);
case Auto_Loiter: case SubMode::Loiter:
return rover.mode_loiter.set_desired_speed(speed); return rover.mode_loiter.set_desired_speed(speed);
case Auto_Guided: case SubMode::Guided:
case Auto_NavScriptTime: case SubMode::NavScriptTime:
return rover.mode_guided.set_desired_speed(speed); return rover.mode_guided.set_desired_speed(speed);
case Auto_Circle: case SubMode::Circle:
return rover.g2.mode_circle.set_desired_speed(speed); return rover.g2.mode_circle.set_desired_speed(speed);
} }
return false; return false;
@ -381,7 +381,7 @@ bool ModeAuto::set_desired_speed(float speed)
void ModeAuto::start_RTL() void ModeAuto::start_RTL()
{ {
if (rover.mode_rtl.enter()) { if (rover.mode_rtl.enter()) {
_submode = Auto_RTL; _submode = SubMode::RTL;
} }
} }
@ -389,7 +389,7 @@ void ModeAuto::start_RTL()
bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4)
{ {
#if AP_SCRIPTING_ENABLED #if AP_SCRIPTING_ENABLED
if (_submode == AutoSubMode::Auto_NavScriptTime) { if (_submode == SubMode::NavScriptTime) {
id = nav_scripting.id; id = nav_scripting.id;
cmd = nav_scripting.command; cmd = nav_scripting.command;
arg1 = nav_scripting.arg1; arg1 = nav_scripting.arg1;
@ -406,7 +406,7 @@ bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &a
void ModeAuto::nav_script_time_done(uint16_t id) void ModeAuto::nav_script_time_done(uint16_t id)
{ {
#if AP_SCRIPTING_ENABLED #if AP_SCRIPTING_ENABLED
if ((_submode == AutoSubMode::Auto_NavScriptTime) && (id == nav_scripting.id)) { if ((_submode == SubMode::NavScriptTime) && (id == nav_scripting.id)) {
nav_scripting.done = true; nav_scripting.done = true;
} }
#endif #endif
@ -458,7 +458,7 @@ bool ModeAuto::check_trigger(void)
bool ModeAuto::start_loiter() bool ModeAuto::start_loiter()
{ {
if (rover.mode_loiter.enter()) { if (rover.mode_loiter.enter()) {
_submode = Auto_Loiter; _submode = SubMode::Loiter;
return true; return true;
} }
return false; return false;
@ -468,7 +468,7 @@ bool ModeAuto::start_loiter()
void ModeAuto::start_guided(const Location& loc) void ModeAuto::start_guided(const Location& loc)
{ {
if (rover.mode_guided.enter()) { if (rover.mode_guided.enter()) {
_submode = Auto_Guided; _submode = SubMode::Guided;
// initialise guided start time and position as reference for limit checking // initialise guided start time and position as reference for limit checking
rover.mode_guided.limit_init_time_and_location(); rover.mode_guided.limit_init_time_and_location();
@ -487,7 +487,7 @@ void ModeAuto::start_guided(const Location& loc)
// start stopping vehicle as quickly as possible // start stopping vehicle as quickly as possible
void ModeAuto::start_stop() void ModeAuto::start_stop()
{ {
_submode = Auto_Stop; _submode = SubMode::Stop;
} }
// send latest position target to offboard navigation system // send latest position target to offboard navigation system
@ -820,7 +820,7 @@ void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
_desired_speed = constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max); _desired_speed = constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max);
_desired_yaw_cd = desired_heading_cd; _desired_yaw_cd = desired_heading_cd;
_reached_heading = false; _reached_heading = false;
_submode = Auto_HeadingAndSpeed; _submode = SubMode::HeadingAndSpeed;
} }
/********************************************************************************/ /********************************************************************************/
@ -896,7 +896,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{ {
// if we failed to enter guided or this command disables guided // if we failed to enter guided or this command disables guided
// return true so we move to next command // return true so we move to next command
if (_submode != Auto_Guided || cmd.p1 == 0) { if (_submode != SubMode::Guided || cmd.p1 == 0) {
return true; return true;
} }
@ -914,7 +914,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
// verify_yaw - return true if we have reached the desired heading // verify_yaw - return true if we have reached the desired heading
bool ModeAuto::verify_nav_set_yaw_speed() bool ModeAuto::verify_nav_set_yaw_speed()
{ {
if (_submode == Auto_HeadingAndSpeed) { if (_submode == SubMode::HeadingAndSpeed) {
return _reached_heading; return _reached_heading;
} }
// we should never reach here but just in case, return true to allow missions to continue // we should never reach here but just in case, return true to allow missions to continue
@ -937,7 +937,7 @@ bool ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
// initialise circle mode // initialise circle mode
if (g2.mode_circle.set_center(circle_center, circle_radius_m, cmd.content.location.loiter_ccw)) { if (g2.mode_circle.set_center(circle_center, circle_radius_m, cmd.content.location.loiter_ccw)) {
_submode = Auto_Circle; _submode = SubMode::Circle;
return true; return true;
} }
return false; return false;
@ -1031,7 +1031,7 @@ void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
{ {
// call regular guided flight mode initialisation // call regular guided flight mode initialisation
if (rover.mode_guided.enter()) { if (rover.mode_guided.enter()) {
_submode = AutoSubMode::Auto_NavScriptTime; _submode = SubMode::NavScriptTime;
nav_scripting.done = false; nav_scripting.done = false;
nav_scripting.id++; nav_scripting.id++;
nav_scripting.start_ms = millis(); nav_scripting.start_ms = millis();