Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: make guided submode enum class

Browse Source
This commit is contained in:
Pierre Kancir 2023-09-21 15:44:53 +02:00 committed by Peter Barker
parent d36667c84a
commit f088f0df9e
2 changed files with 73 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Rover/mode.h
View File

@ -544,13 +544,13 @@ public:
protected: protected:
enum GuidedMode { enum class SubMode: uint8_t {
Guided_WP, WP,
Guided_HeadingAndSpeed, HeadingAndSpeed,
Guided_TurnRateAndSpeed, TurnRateAndSpeed,
Guided_Loiter, Loiter,
Guided_SteeringAndThrottle, SteeringAndThrottle,
Guided_Stop Stop
}; };
// enum for GUID_OPTIONS parameter // enum for GUID_OPTIONS parameter
@ -564,7 +564,7 @@ protected:
// scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping) // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping)
bool use_scurves_for_navigation() const; bool use_scurves_for_navigation() const;
GuidedMode _guided_mode; // stores which GUIDED mode the vehicle is in SubMode _guided_mode; // stores which GUIDED mode the vehicle is in
// attitude control // attitude control
bool have_attitude_target; // true if we have a valid attitude target bool have_attitude_target; // true if we have a valid attitude target

130
Rover/mode_guided.cpp
View File

@ -22,7 +22,7 @@ bool ModeGuided::_enter()
void ModeGuided::update() void ModeGuided::update()
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case SubMode::WP:
{ {
// check if we've reached the destination // check if we've reached the destination
if (!g2.wp_nav.reached_destination()) { if (!g2.wp_nav.reached_destination()) {
@ -49,7 +49,7 @@ void ModeGuided::update()
break; break;
} }
case Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
{ {
// stop vehicle if target not updated within 3 seconds // stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
@ -73,7 +73,7 @@ void ModeGuided::update()
break; break;
} }
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
{ {
// stop vehicle if target not updated within 3 seconds // stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
@ -101,13 +101,13 @@ void ModeGuided::update()
break; break;
} }
case Guided_Loiter: case SubMode::Loiter:
{ {
rover.mode_loiter.update(); rover.mode_loiter.update();
break; break;
} }
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
{ {
// handle timeout // handle timeout
if (_have_strthr && (AP_HAL::millis() - _strthr_time_ms) > 3000) { if (_have_strthr && (AP_HAL::millis() - _strthr_time_ms) > 3000) {
@ -131,7 +131,7 @@ void ModeGuided::update()
break; break;
} }
case Guided_Stop: case SubMode::Stop:
stop_vehicle(); stop_vehicle();
break; break;
@ -145,15 +145,15 @@ void ModeGuided::update()
float ModeGuided::wp_bearing() const float ModeGuided::wp_bearing() const
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case SubMode::WP:
return g2.wp_nav.wp_bearing_cd() * 0.01f; return g2.wp_nav.wp_bearing_cd() * 0.01f;
case Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
return 0.0f; return 0.0f;
case Guided_Loiter: case SubMode::Loiter:
return rover.mode_loiter.wp_bearing(); return rover.mode_loiter.wp_bearing();
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
case Guided_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
} }
@ -164,15 +164,15 @@ float ModeGuided::wp_bearing() const
float ModeGuided::nav_bearing() const float ModeGuided::nav_bearing() const
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case SubMode::WP:
return g2.wp_nav.nav_bearing_cd() * 0.01f; return g2.wp_nav.nav_bearing_cd() * 0.01f;
case Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
return 0.0f; return 0.0f;
case Guided_Loiter: case SubMode::Loiter:
return rover.mode_loiter.nav_bearing(); return rover.mode_loiter.nav_bearing();
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
case Guided_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
} }
@ -183,15 +183,15 @@ float ModeGuided::nav_bearing() const
float ModeGuided::crosstrack_error() const float ModeGuided::crosstrack_error() const
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case SubMode::WP:
return g2.wp_nav.crosstrack_error(); return g2.wp_nav.crosstrack_error();
case Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
return 0.0f; return 0.0f;
case Guided_Loiter: case SubMode::Loiter:
return rover.mode_loiter.crosstrack_error(); return rover.mode_loiter.crosstrack_error();
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
case Guided_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
} }
@ -202,15 +202,15 @@ float ModeGuided::crosstrack_error() const
float ModeGuided::get_desired_lat_accel() const float ModeGuided::get_desired_lat_accel() const
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case SubMode::WP:
return g2.wp_nav.get_lat_accel(); return g2.wp_nav.get_lat_accel();
case Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
return 0.0f; return 0.0f;
case Guided_Loiter: case SubMode::Loiter:
return rover.mode_loiter.get_desired_lat_accel(); return rover.mode_loiter.get_desired_lat_accel();
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
case Guided_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
} }
@ -222,15 +222,15 @@ float ModeGuided::get_desired_lat_accel() const
float ModeGuided::get_distance_to_destination() const float ModeGuided::get_distance_to_destination() const
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case SubMode::WP:
return _distance_to_destination; return _distance_to_destination;
case Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
return 0.0f; return 0.0f;
case Guided_Loiter: case SubMode::Loiter:
return rover.mode_loiter.get_distance_to_destination(); return rover.mode_loiter.get_distance_to_destination();
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
case Guided_Stop: case SubMode::Stop:
return 0.0f; return 0.0f;
} }
@ -242,13 +242,13 @@ float ModeGuided::get_distance_to_destination() const
bool ModeGuided::reached_destination() const bool ModeGuided::reached_destination() const
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case SubMode::WP:
return g2.wp_nav.reached_destination(); return g2.wp_nav.reached_destination();
case Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
case Guided_Loiter: case SubMode::Loiter:
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
case Guided_Stop: case SubMode::Stop:
return true; return true;
} }
@ -260,16 +260,16 @@ bool ModeGuided::reached_destination() const
bool ModeGuided::set_desired_speed(float speed) bool ModeGuided::set_desired_speed(float speed)
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_WP: case SubMode::WP:
return g2.wp_nav.set_speed_max(speed); return g2.wp_nav.set_speed_max(speed);
case Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
// speed is set from mavlink message // speed is set from mavlink message
return false; return false;
case Guided_Loiter: case SubMode::Loiter:
return rover.mode_loiter.set_desired_speed(speed); return rover.mode_loiter.set_desired_speed(speed);
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
case Guided_Stop: case SubMode::Stop:
// no speed control // no speed control
return false; return false;
} }
@ -280,21 +280,21 @@ bool ModeGuided::set_desired_speed(float speed)
bool ModeGuided::get_desired_location(Location& destination) const bool ModeGuided::get_desired_location(Location& destination) const
{ {
switch (_guided_mode) { switch (_guided_mode) {
case Guided_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 Guided_HeadingAndSpeed: case SubMode::HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case SubMode::TurnRateAndSpeed:
// not supported in these submodes // not supported in these submodes
return false; return false;
case Guided_Loiter: case SubMode::Loiter:
// get destination from loiter // get destination from loiter
return rover.mode_loiter.get_desired_location(destination); return rover.mode_loiter.get_desired_location(destination);
case Guided_SteeringAndThrottle: case SubMode::SteeringAndThrottle:
case Guided_Stop: case SubMode::Stop:
// no desired location in this submode // no desired location in this submode
break; break;
} }
@ -320,9 +320,9 @@ bool ModeGuided::set_desired_location(const Location &destination, Location next
} }
// handle guided specific initialisation and logging // handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP; _guided_mode = SubMode::WP;
send_notification = true; send_notification = true;
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f)); rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f));
return true; return true;
} }
@ -330,7 +330,7 @@ bool ModeGuided::set_desired_location(const Location &destination, Location next
void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
{ {
// initialisation and logging // initialisation and logging
_guided_mode = ModeGuided::Guided_HeadingAndSpeed; _guided_mode = SubMode::HeadingAndSpeed;
_des_att_time_ms = AP_HAL::millis(); _des_att_time_ms = AP_HAL::millis();
// record targets // record targets
@ -339,14 +339,14 @@ void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_
have_attitude_target = true; have_attitude_target = true;
// log new target // log new target
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
} }
void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed) void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed)
{ {
// handle initialisation // handle initialisation
if (_guided_mode != ModeGuided::Guided_HeadingAndSpeed) { if (_guided_mode != SubMode::HeadingAndSpeed) {
_guided_mode = ModeGuided::Guided_HeadingAndSpeed; _guided_mode = SubMode::HeadingAndSpeed;
_desired_yaw_cd = ahrs.yaw_sensor; _desired_yaw_cd = ahrs.yaw_sensor;
} }
set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed); set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed);
@ -356,7 +356,7 @@ void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float t
void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed) void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed)
{ {
// handle initialisation // handle initialisation
_guided_mode = ModeGuided::Guided_TurnRateAndSpeed; _guided_mode = SubMode::TurnRateAndSpeed;
_des_att_time_ms = AP_HAL::millis(); _des_att_time_ms = AP_HAL::millis();
// record targets // record targets
@ -365,13 +365,13 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ
have_attitude_target = true; have_attitude_target = true;
// log new target // log new target
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
} }
// set steering and throttle (both in the range -1 to +1) // set steering and throttle (both in the range -1 to +1)
void ModeGuided::set_steering_and_throttle(float steering, float throttle) void ModeGuided::set_steering_and_throttle(float steering, float throttle)
{ {
_guided_mode = ModeGuided::Guided_SteeringAndThrottle; _guided_mode = SubMode::SteeringAndThrottle;
_strthr_time_ms = AP_HAL::millis(); _strthr_time_ms = AP_HAL::millis();
_strthr_steering = constrain_float(steering, -1.0f, 1.0f); _strthr_steering = constrain_float(steering, -1.0f, 1.0f);
_strthr_throttle = constrain_float(throttle, -1.0f, 1.0f); _strthr_throttle = constrain_float(throttle, -1.0f, 1.0f);
@ -381,7 +381,7 @@ void ModeGuided::set_steering_and_throttle(float steering, float throttle)
bool ModeGuided::start_loiter() bool ModeGuided::start_loiter()
{ {
if (rover.mode_loiter.enter()) { if (rover.mode_loiter.enter()) {
_guided_mode = Guided_Loiter; _guided_mode = SubMode::Loiter;
return true; return true;
} }
return false; return false;
@ -391,7 +391,7 @@ bool ModeGuided::start_loiter()
// start stopping vehicle as quickly as possible // start stopping vehicle as quickly as possible
void ModeGuided::start_stop() void ModeGuided::start_stop()
{ {
_guided_mode = Guided_Stop; _guided_mode = SubMode::Stop;
} }
// set guided timeout and movement limits // set guided timeout and movement limits