From c7f3d0046de5f3c70a172c03ba58a35502af1550 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 21 Sep 2023 15:56:25 +0200 Subject: [PATCH] Rover: make Auto submode enum class --- Rover/mode.h | 20 +++--- Rover/mode_auto.cpp | 170 ++++++++++++++++++++++---------------------- 2 files changed, 95 insertions(+), 95 deletions(-) diff --git a/Rover/mode.h b/Rover/mode.h index 3aa0cfa324..342e4704ed 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -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: diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 5be1d1eba0..e71264a7ed 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -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();