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; }
// 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)
float wp_bearing() const override;
@ -294,15 +294,15 @@ protected:
bool _enter() override;
void _exit() override;
enum AutoSubMode {
Auto_WP, // drive to a given location
Auto_HeadingAndSpeed, // turn to a given heading
Auto_RTL, // perform RTL within auto mode
Auto_Loiter, // perform Loiter within auto mode
Auto_Guided, // handover control to external navigation system from within auto mode
Auto_Stop, // stop the vehicle as quickly as possible
Auto_NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing
Auto_Circle, // circle a given location
enum SubMode: uint8_t {
WP, // drive to a given location
HeadingAndSpeed, // turn to a given heading
RTL, // perform RTL within auto mode
Loiter, // perform Loiter within auto mode
Guided, // handover control to external navigation system from within auto mode
Stop, // stop the vehicle as quickly as possible
NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing
Circle, // circle a given location
} _submode;
private:

170
Rover/mode_auto.cpp
View File

@ -66,7 +66,7 @@ void ModeAuto::update()
// check for mission changes
if (mis_change_detector.check_for_mission_change()) {
// 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()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
} else {
@ -80,7 +80,7 @@ void ModeAuto::update()
}
switch (_submode) {
case Auto_WP:
case SubMode::WP:
{
// check if we've reached the destination
if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) {
@ -101,7 +101,7 @@ void ModeAuto::update()
break;
}
case Auto_HeadingAndSpeed:
case SubMode::HeadingAndSpeed:
{
if (!_reached_heading) {
// run steering and throttle controllers
@ -122,15 +122,15 @@ void ModeAuto::update()
break;
}
case Auto_RTL:
case SubMode::RTL:
rover.mode_rtl.update();
break;
case Auto_Loiter:
case SubMode::Loiter:
rover.mode_loiter.update();
break;
case Auto_Guided:
case SubMode::Guided:
{
// send location target to offboard navigation system
send_guided_position_target();
@ -138,15 +138,15 @@ void ModeAuto::update()
break;
}
case Auto_Stop:
case SubMode::Stop:
stop_vehicle();
break;
case Auto_NavScriptTime:
case SubMode::NavScriptTime:
rover.mode_guided.update();
break;
case Auto_Circle:
case SubMode::Circle:
rover.g2.mode_circle.update();
break;
}
@ -166,19 +166,19 @@ void ModeAuto::calc_throttle(float target_speed, bool avoidance_enabled)
float ModeAuto::wp_bearing() const
{
switch (_submode) {
case Auto_WP:
case SubMode::WP:
return g2.wp_nav.wp_bearing_cd() * 0.01f;
case Auto_HeadingAndSpeed:
case Auto_Stop:
case SubMode::HeadingAndSpeed:
case SubMode::Stop:
return 0.0f;
case Auto_RTL:
case SubMode::RTL:
return rover.mode_rtl.wp_bearing();
case Auto_Loiter:
case SubMode::Loiter:
return rover.mode_loiter.wp_bearing();
case Auto_Guided:
case Auto_NavScriptTime:
case SubMode::Guided:
case SubMode::NavScriptTime:
return rover.mode_guided.wp_bearing();
case Auto_Circle:
case SubMode::Circle:
return rover.g2.mode_circle.wp_bearing();
}
@ -190,19 +190,19 @@ float ModeAuto::wp_bearing() const
float ModeAuto::nav_bearing() const
{
switch (_submode) {
case Auto_WP:
case SubMode::WP:
return g2.wp_nav.nav_bearing_cd() * 0.01f;
case Auto_HeadingAndSpeed:
case Auto_Stop:
case SubMode::HeadingAndSpeed:
case SubMode::Stop:
return 0.0f;
case Auto_RTL:
case SubMode::RTL:
return rover.mode_rtl.nav_bearing();
case Auto_Loiter:
case SubMode::Loiter:
return rover.mode_loiter.nav_bearing();
case Auto_Guided:
case Auto_NavScriptTime:
case SubMode::Guided:
case SubMode::NavScriptTime:
return rover.mode_guided.nav_bearing();
case Auto_Circle:
case SubMode::Circle:
return rover.g2.mode_circle.nav_bearing();
}
@ -214,19 +214,19 @@ float ModeAuto::nav_bearing() const
float ModeAuto::crosstrack_error() const
{
switch (_submode) {
case Auto_WP:
case SubMode::WP:
return g2.wp_nav.crosstrack_error();
case Auto_HeadingAndSpeed:
case Auto_Stop:
case SubMode::HeadingAndSpeed:
case SubMode::Stop:
return 0.0f;
case Auto_RTL:
case SubMode::RTL:
return rover.mode_rtl.crosstrack_error();
case Auto_Loiter:
case SubMode::Loiter:
return rover.mode_loiter.crosstrack_error();
case Auto_Guided:
case Auto_NavScriptTime:
case SubMode::Guided:
case SubMode::NavScriptTime:
return rover.mode_guided.crosstrack_error();
case Auto_Circle:
case SubMode::Circle:
return rover.g2.mode_circle.crosstrack_error();
}
@ -238,19 +238,19 @@ float ModeAuto::crosstrack_error() const
float ModeAuto::get_desired_lat_accel() const
{
switch (_submode) {
case Auto_WP:
case SubMode::WP:
return g2.wp_nav.get_lat_accel();
case Auto_HeadingAndSpeed:
case Auto_Stop:
case SubMode::HeadingAndSpeed:
case SubMode::Stop:
return 0.0f;
case Auto_RTL:
case SubMode::RTL:
return rover.mode_rtl.get_desired_lat_accel();
case Auto_Loiter:
case SubMode::Loiter:
return rover.mode_loiter.get_desired_lat_accel();
case Auto_Guided:
case Auto_NavScriptTime:
case SubMode::Guided:
case SubMode::NavScriptTime:
return rover.mode_guided.get_desired_lat_accel();
case Auto_Circle:
case SubMode::Circle:
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
{
switch (_submode) {
case Auto_WP:
case SubMode::WP:
return _distance_to_destination;
case Auto_HeadingAndSpeed:
case Auto_Stop:
case SubMode::HeadingAndSpeed:
case SubMode::Stop:
// no valid distance so return zero
return 0.0f;
case Auto_RTL:
case SubMode::RTL:
return rover.mode_rtl.get_distance_to_destination();
case Auto_Loiter:
case SubMode::Loiter:
return rover.mode_loiter.get_distance_to_destination();
case Auto_Guided:
case Auto_NavScriptTime:
case SubMode::Guided:
case SubMode::NavScriptTime:
return rover.mode_guided.get_distance_to_destination();
case Auto_Circle:
case SubMode::Circle:
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
{
switch (_submode) {
case Auto_WP:
case SubMode::WP:
if (g2.wp_nav.is_destination_valid()) {
destination = g2.wp_nav.get_oa_destination();
return true;
}
return false;
case Auto_HeadingAndSpeed:
case Auto_Stop:
case SubMode::HeadingAndSpeed:
case SubMode::Stop:
// no desired location for this submode
return false;
case Auto_RTL:
case SubMode::RTL:
return rover.mode_rtl.get_desired_location(destination);
case Auto_Loiter:
case SubMode::Loiter:
return rover.mode_loiter.get_desired_location(destination);
case Auto_Guided:
case Auto_NavScriptTime:
case SubMode::Guided:
case SubMode::NavScriptTime:
return rover.mode_guided.get_desired_location(destination);
case Auto_Circle:
case SubMode::Circle:
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;
}
_submode = Auto_WP;
_submode = SubMode::WP;
return true;
}
@ -329,24 +329,24 @@ bool ModeAuto::set_desired_location(const Location &destination, Location next_d
bool ModeAuto::reached_destination() const
{
switch (_submode) {
case Auto_WP:
case SubMode::WP:
return g2.wp_nav.reached_destination();
break;
case Auto_HeadingAndSpeed:
case Auto_Stop:
case SubMode::HeadingAndSpeed:
case SubMode::Stop:
// always return true because this is the safer option to allow missions to continue
return true;
break;
case Auto_RTL:
case SubMode::RTL:
return rover.mode_rtl.reached_destination();
break;
case Auto_Loiter:
case SubMode::Loiter:
return rover.mode_loiter.reached_destination();
break;
case Auto_Guided:
case Auto_NavScriptTime:
case SubMode::Guided:
case SubMode::NavScriptTime:
return rover.mode_guided.reached_destination();
case Auto_Circle:
case SubMode::Circle:
return rover.g2.mode_circle.reached_destination();
}
@ -358,20 +358,20 @@ bool ModeAuto::reached_destination() const
bool ModeAuto::set_desired_speed(float speed)
{
switch (_submode) {
case Auto_WP:
case Auto_Stop:
case SubMode::WP:
case SubMode::Stop:
return g2.wp_nav.set_speed_max(speed);
case Auto_HeadingAndSpeed:
case SubMode::HeadingAndSpeed:
_desired_speed = speed;
return true;
case Auto_RTL:
case SubMode::RTL:
return rover.mode_rtl.set_desired_speed(speed);
case Auto_Loiter:
case SubMode::Loiter:
return rover.mode_loiter.set_desired_speed(speed);
case Auto_Guided:
case Auto_NavScriptTime:
case SubMode::Guided:
case SubMode::NavScriptTime:
return rover.mode_guided.set_desired_speed(speed);
case Auto_Circle:
case SubMode::Circle:
return rover.g2.mode_circle.set_desired_speed(speed);
}
return false;
@ -381,7 +381,7 @@ bool ModeAuto::set_desired_speed(float speed)
void ModeAuto::start_RTL()
{
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)
{
#if AP_SCRIPTING_ENABLED
if (_submode == AutoSubMode::Auto_NavScriptTime) {
if (_submode == SubMode::NavScriptTime) {
id = nav_scripting.id;
cmd = nav_scripting.command;
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)
{
#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;
}
#endif
@ -458,7 +458,7 @@ bool ModeAuto::check_trigger(void)
bool ModeAuto::start_loiter()
{
if (rover.mode_loiter.enter()) {
_submode = Auto_Loiter;
_submode = SubMode::Loiter;
return true;
}
return false;
@ -468,7 +468,7 @@ bool ModeAuto::start_loiter()
void ModeAuto::start_guided(const Location& loc)
{
if (rover.mode_guided.enter()) {
_submode = Auto_Guided;
_submode = SubMode::Guided;
// initialise guided start time and position as reference for limit checking
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
void ModeAuto::start_stop()
{
_submode = Auto_Stop;
_submode = SubMode::Stop;
}
// 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_yaw_cd = desired_heading_cd;
_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
// return true so we move to next command
if (_submode != Auto_Guided || cmd.p1 == 0) {
if (_submode != SubMode::Guided || cmd.p1 == 0) {
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
bool ModeAuto::verify_nav_set_yaw_speed()
{
if (_submode == Auto_HeadingAndSpeed) {
if (_submode == SubMode::HeadingAndSpeed) {
return _reached_heading;
}
// 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
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 false;
@ -1031,7 +1031,7 @@ void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
{
// call regular guided flight mode initialisation
if (rover.mode_guided.enter()) {
_submode = AutoSubMode::Auto_NavScriptTime;
_submode = SubMode::NavScriptTime;
nav_scripting.done = false;
nav_scripting.id++;
nav_scripting.start_ms = millis();