rover: improve hold position logic (#25466)

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());
+			speed_setpoint = math::min(speed_setpoint, _cruising_speed);
-
-			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));
-			}
 
 			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);
-
+		_updated_reset_counter = vehicle_local_position.xy_reset_counter;
-		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);
-		}
-
 		_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);
+		rover_position_setpoint_s rover_position_setpoint;
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		_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;
 	}
 
 }

src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp

@@ -41,7 +41,6 @@
 #include <matrix/matrix/math.hpp>
 #include <lib/pure_pursuit/PurePursuit.hpp>
 #include <math.h>
-#include <lib/geo/geo.h>
 
 // uORB includes
 #include <uORB/Publication.hpp>
@@ -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_s> _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};
-
+	float _vehicle_speed{0.f};
-	// Class Instances
+	float _cruising_speed{NAN};
-	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
+	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<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
@@ -128,6 +134,7 @@ private:
 		(ParamFloat<px4::params::PP_LOOKAHD_MIN>)   _param_pp_lookahd_min,
 		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
 		(ParamFloat<px4::params::RA_WHEEL_BASE>)    _param_ra_wheel_base,
-		(ParamFloat<px4::params::RA_MAX_STR_ANG>)   _param_ra_max_str_ang
+		(ParamFloat<px4::params::RA_MAX_STR_ANG>)   _param_ra_max_str_ang,
+		(ParamFloat<px4::params::RO_SPEED_TH>)      _param_ro_speed_th
 	)
 };

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();

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()) {
+	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 -=
@@ -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());
+			speed_setpoint = math::min(speed_setpoint, _cruising_speed);
-
-			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));
-			}
 
 			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_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
+		_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);
+		rover_position_setpoint_s rover_position_setpoint;
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		_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()

src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp

@@ -38,7 +38,6 @@
 #include <px4_platform_common/events.h>
 
 // Libraries
-#include <lib/rover_control/RoverControl.hpp>
 #include <matrix/matrix/math.hpp>
 #include <lib/pure_pursuit/PurePursuit.hpp>
 #include <math.h>
@@ -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_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
@@ -111,10 +114,17 @@ private:
 	uORB::Publication<rover_attitude_setpoint_s> _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<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max,
 		(ParamFloat<px4::params::PP_LOOKAHD_MIN>)   _param_pp_lookahd_min,
 		(ParamFloat<px4::params::NAV_ACC_RAD>)      _param_nav_acc_rad,
-		(ParamFloat<px4::params::RO_SPEED_RED>)     _param_ro_speed_red
+		(ParamFloat<px4::params::RO_SPEED_RED>)     _param_ro_speed_red,
+		(ParamFloat<px4::params::RO_SPEED_TH>)      _param_ro_speed_th
 	)
 };

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();

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());
+			speed_setpoint = math::min(speed_setpoint, _cruising_speed);
-
-			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));
-			}
 
 			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);
-
+		_updated_reset_counter = vehicle_local_position.xy_reset_counter;
-		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);
-		}
-
 		_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);
+		rover_position_setpoint_s rover_position_setpoint;
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		_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()

src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp

@@ -38,7 +38,6 @@
 #include <px4_platform_common/events.h>
 
 // Libraries
-#include <lib/rover_control/RoverControl.hpp>
 #include <matrix/matrix/math.hpp>
 #include <lib/pure_pursuit/PurePursuit.hpp>
 #include <lib/geo/geo.h>
@@ -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_s>    _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};
-
+	float _vehicle_speed{0.f};
-	// Class Instances
+	float _cruising_speed{NAN};
-	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
+	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<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th,

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();