From 248f11314161bd8ee5c3b8cd468405c0d99581a1 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Fri, 22 Aug 2025 12:10:16 +0200 Subject: [PATCH] rover: improve hold position logic (#25466) --- .../AckermannPosControl.cpp | 48 ++++++++++-------- .../AckermannPosControl.hpp | 19 ++++--- .../rover_ackermann/RoverAckermann.cpp | 1 + .../DifferentialPosControl.cpp | 48 +++++++++++------- .../DifferentialPosControl.hpp | 17 +++++-- .../rover_differential/RoverDifferential.cpp | 1 + .../MecanumPosControl/MecanumPosControl.cpp | 50 ++++++++++--------- .../MecanumPosControl/MecanumPosControl.hpp | 18 +++++-- src/modules/rover_mecanum/RoverMecanum.cpp | 1 + 9 files changed, 128 insertions(+), 75 deletions(-) diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index d59a87c232..dc8f2ae136 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -58,10 +58,8 @@ void AckermannPosControl::updatePosControl() hrt_abstime timestamp = hrt_absolute_time(); - const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]); - - if (target_waypoint_ned.isAllFinite()) { - float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + if (_target_waypoint_ned.isAllFinite()) { + float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm(); if (_arrival_speed > FLT_EPSILON) { distance_to_target -= _acceptance_radius; // shift target to the edge of the acceptance radius if arrival speed not zero @@ -71,18 +69,13 @@ void AckermannPosControl::updatePosControl() float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); - speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); - - if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { - speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint, - fabsf(_rover_position_setpoint.cruising_speed)); - } + speed_setpoint = math::min(speed_setpoint, _cruising_speed); pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = timestamp; const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned, + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned, _curr_pos_ned, fabsf(speed_setpoint)); if (_param_ro_speed_red.get() > FLT_EPSILON) { @@ -114,6 +107,16 @@ void AckermannPosControl::updatePosControl() rover_attitude_setpoint.timestamp = timestamp; rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) { + _stopped = true; + _target_waypoint_ned = _curr_pos_ned; + } + + if (_stopped && _updated_reset_counter != _reset_counter) { + _target_waypoint_ned = _curr_pos_ned; + _reset_counter = _updated_reset_counter; + } } } } @@ -136,21 +139,24 @@ void AckermannPosControl::updateSubscriptions() if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, - vehicle_local_position.ref_timestamp); - } - + _updated_reset_counter = vehicle_local_position.xy_reset_counter; _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); + _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } if (_rover_position_setpoint_sub.updated()) { - _rover_position_setpoint_sub.copy(&_rover_position_setpoint); - _start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]); + rover_position_setpoint_s rover_position_setpoint; + _rover_position_setpoint_sub.copy(&rover_position_setpoint); + _start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]); _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; - _arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f; + _arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f; + _cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed : + _param_ro_speed_limit.get(); + _target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]); + _stopped = false; } } diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp index f43cc5b172..e67f83440a 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp @@ -41,7 +41,6 @@ #include #include #include -#include // uORB includes #include @@ -80,6 +79,11 @@ public: */ bool runSanityChecks(); + /** + * @brief Reset position controller. + */ + void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;}; + protected: /** * @brief Update the parameters of the module. @@ -97,7 +101,6 @@ private: uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)}; - rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; @@ -108,14 +111,17 @@ private: Quatf _vehicle_attitude_quaternion{}; Vector2f _curr_pos_ned{}; Vector2f _start_ned{}; + Vector2f _target_waypoint_ned{}; float _arrival_speed{0.f}; float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; float _acceptance_radius{0.f}; // Acceptance radius for the waypoint. float _min_speed{NAN}; - - // Class Instances - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + float _vehicle_speed{0.f}; + float _cruising_speed{NAN}; + bool _stopped{false}; + uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */ DEFINE_PARAMETERS( (ParamFloat) _param_ro_max_thr_speed, @@ -128,6 +134,7 @@ private: (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_ra_max_str_ang + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_speed_th ) }; diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index fa7b814134..fe85aa1b90 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -182,6 +182,7 @@ void RoverAckermann::runSanityChecks() void RoverAckermann::reset() { + _ackermann_pos_control.reset(); _ackermann_speed_control.reset(); _ackermann_att_control.reset(); _ackermann_rate_control.reset(); diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index e9b7a9ae44..d7807c2e8d 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -55,10 +55,9 @@ void DifferentialPosControl::updatePosControl() hrt_abstime timestamp = hrt_absolute_time(); - const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]); - if (target_waypoint_ned.isAllFinite()) { - float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + if (_target_waypoint_ned.isAllFinite()) { + float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm(); if (_arrival_speed > FLT_EPSILON) { distance_to_target -= @@ -68,18 +67,13 @@ void DifferentialPosControl::updatePosControl() if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) { float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); - speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); - - if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { - speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint, - fabsf(_rover_position_setpoint.cruising_speed)); - } + speed_setpoint = math::min(speed_setpoint, _cruising_speed); pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = timestamp; const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned, + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned, _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); @@ -122,6 +116,16 @@ void DifferentialPosControl::updatePosControl() rover_attitude_setpoint.timestamp = timestamp; rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) { + _stopped = true; + _target_waypoint_ned = _curr_pos_ned; + } + + if (_stopped && _updated_reset_counter != _reset_counter) { + _target_waypoint_ned = _curr_pos_ned; + _reset_counter = _updated_reset_counter; + } } } @@ -132,24 +136,34 @@ void DifferentialPosControl::updateSubscriptions() if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); - matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); } if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); + _updated_reset_counter = vehicle_local_position.xy_reset_counter; + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + + Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); + _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } if (_rover_position_setpoint_sub.updated()) { - _rover_position_setpoint_sub.copy(&_rover_position_setpoint); - _start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]); + rover_position_setpoint_s rover_position_setpoint; + _rover_position_setpoint_sub.copy(&rover_position_setpoint); + _start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]); _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; - _arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f; + _arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f; + _cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed : + _param_ro_speed_limit.get(); + _target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]); + _stopped = false; } - - } bool DifferentialPosControl::runSanityChecks() diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index 5ebe7d5a12..fde8a25515 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -38,7 +38,6 @@ #include // Libraries -#include #include #include #include @@ -87,6 +86,11 @@ public: */ bool runSanityChecks(); + /** + * @brief Reset position controller. + */ + void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;}; + protected: /** * @brief Update the parameters of the module. @@ -103,7 +107,6 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; - rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; @@ -111,10 +114,17 @@ private: uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; // Variables + Quatf _vehicle_attitude_quaternion{}; Vector2f _curr_pos_ned{}; Vector2f _start_ned{}; + Vector2f _target_waypoint_ned{}; float _arrival_speed{0.f}; float _vehicle_yaw{0.f}; + float _vehicle_speed{0.f}; + float _cruising_speed{NAN}; + bool _stopped{false}; + uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */ DrivingState _current_state{DrivingState::DRIVING}; DEFINE_PARAMETERS( @@ -128,6 +138,7 @@ private: (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_ro_speed_red + (ParamFloat) _param_ro_speed_red, + (ParamFloat) _param_ro_speed_th ) }; diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 5efd3e94dc..e41e01f260 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -183,6 +183,7 @@ void RoverDifferential::runSanityChecks() void RoverDifferential::reset() { + _differential_pos_control.reset(); _differential_speed_control.reset(); _differential_att_control.reset(); _differential_rate_control.reset(); diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index b30403ba61..3ec9be67d3 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -57,11 +57,9 @@ void MecanumPosControl::updatePosControl() hrt_abstime timestamp = hrt_absolute_time(); - const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]); + if (_target_waypoint_ned.isAllFinite()) { - if (target_waypoint_ned.isAllFinite()) { - - float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm(); if (_arrival_speed > FLT_EPSILON) { distance_to_target -= @@ -72,18 +70,13 @@ void MecanumPosControl::updatePosControl() float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); - speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); - - if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { - speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint, - fabsf(_rover_position_setpoint.cruising_speed)); - } + speed_setpoint = math::min(speed_setpoint, _cruising_speed); pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = timestamp; const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned, + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned, _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); @@ -111,6 +104,16 @@ void MecanumPosControl::updatePosControl() rover_attitude_setpoint.timestamp = timestamp; rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) { + _stopped = true; + _target_waypoint_ned = _curr_pos_ned; + } + + if (_stopped && _updated_reset_counter != _reset_counter) { + _target_waypoint_ned = _curr_pos_ned; + _reset_counter = _updated_reset_counter; + } } } } @@ -127,24 +130,25 @@ void MecanumPosControl::updateSubscriptions() if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, - vehicle_local_position.ref_timestamp); - } - + _updated_reset_counter = vehicle_local_position.xy_reset_counter; _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); + _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } if (_rover_position_setpoint_sub.updated()) { - _rover_position_setpoint_sub.copy(&_rover_position_setpoint); - _start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]); + rover_position_setpoint_s rover_position_setpoint; + _rover_position_setpoint_sub.copy(&rover_position_setpoint); + _start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]); _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; - _yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw; - _arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f; + _arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f; + _cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed : + _param_ro_speed_limit.get(); + _target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]); + _stopped = false; } - } bool MecanumPosControl::runSanityChecks() diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index aab2d34386..54880657ec 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -38,7 +38,6 @@ #include // Libraries -#include #include #include #include @@ -80,6 +79,13 @@ public: */ bool runSanityChecks(); + /** + * @brief Reset position controller. + */ + void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;}; + +protected: + protected: /** * @brief Update the parameters of the module. @@ -96,7 +102,6 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; - rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; @@ -107,13 +112,16 @@ private: Quatf _vehicle_attitude_quaternion{}; Vector2f _curr_pos_ned{}; Vector2f _start_ned{}; + Vector2f _target_waypoint_ned{}; float _arrival_speed{0.f}; float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; float _yaw_setpoint{NAN}; - - // Class Instances - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + float _vehicle_speed{0.f}; + float _cruising_speed{NAN}; + bool _stopped{false}; + uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */ DEFINE_PARAMETERS( (ParamFloat) _param_rm_course_ctl_th, diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index e3709408d8..fb60ee3127 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -183,6 +183,7 @@ void RoverMecanum::runSanityChecks() void RoverMecanum::reset() { + _mecanum_pos_control.reset(); _mecanum_speed_control.reset(); _mecanum_att_control.reset(); _mecanum_rate_control.reset();