rover: auto mode stopping conditions

src/lib/rover_control/RoverControl.cpp

@@ -215,8 +215,7 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
 }
 
 void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
-				  position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
+				  position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, MapProjection &global_ned_proj_ref)
-				  MapProjection &global_ned_proj_ref)
 {
 	if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
 	    && PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
@@ -241,8 +240,7 @@ void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned,
 		next_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
 
 	} else {
-		next_wp_ned = home_pos.isAllFinite() ? global_ned_proj_ref.project(home_pos(0), home_pos(1)) : Vector2f(NAN,
+		next_wp_ned = Vector2f(NAN, NAN);
-				NAN); // Enables corner slow down with RTL
 	}
 }
 

src/lib/rover_control/RoverControl.hpp

@@ -118,11 +118,10 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
  * @param next_wp_ned Next waypoint in NED frame (Updated by this function)
  * @param position_setpoint_triplet Position Setpoint Triplet
  * @param curr_pos Current position of the rover in global frame
- * @param home_pos Home position in global frame
  * @param global_ned_proj_ref Global to ned projection
  */
 void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
-				  position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
+				  position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned,
 				  MapProjection &global_ned_proj_ref);
 
 /**

src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp

@@ -131,12 +131,6 @@ void AckermannPosVelControl::updateSubscriptions()
 		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
 	}
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-		_vehicle_status_sub.copy(&vehicle_status);
-		_nav_state = vehicle_status.nav_state;
-	}
-
 }
 
 void AckermannPosVelControl::generateAttitudeSetpoint()
@@ -254,7 +248,9 @@ void AckermannPosVelControl::offboardVelocityMode()
 
 void AckermannPosVelControl::autoPositionMode()
 {
-	updateAutoSubscriptions();
+	if (_position_setpoint_triplet_sub.updated()) {
+		updateWaypointsAndAcceptanceRadius();
+	}
 
 	// Distances to waypoints
 	const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
@@ -262,17 +258,25 @@ void AckermannPosVelControl::autoPositionMode()
 	const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
 					       2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
 
-	if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival
+	// Check stopping conditions
-		_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
+	bool auto_stop{false};
+
+	if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+	    || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE
+	    || !_next_wp_ned.isAllFinite()) { // Check stopping conditions
+		auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
 	}
 
-	if (_mission_finished) {
+	if (auto_stop) {
 		_speed_body_x_setpoint = 0.f;
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = _timestamp;
+		rover_rate_setpoint.yaw_rate_setpoint = 0.f;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
 
 	} else { // Regular guidance algorithm
-
 		_speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp,
-					 _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _nav_state,
+					 _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _curr_wp_type,
 					 _waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get());
 		pure_pursuit_status_s pure_pursuit_status{};
 		pure_pursuit_status.timestamp = _timestamp;
@@ -284,25 +288,7 @@ void AckermannPosVelControl::autoPositionMode()
 		rover_attitude_setpoint.timestamp = _timestamp;
 		rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
 		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-	}
-}
 
-void AckermannPosVelControl::updateAutoSubscriptions()
-{
-	if (_home_position_sub.updated()) {
-		home_position_s home_position{};
-		_home_position_sub.copy(&home_position);
-		_home_position = Vector2d(home_position.lat, home_position.lon);
-	}
-
-	if (_position_setpoint_triplet_sub.updated()) {
-		updateWaypointsAndAcceptanceRadius();
-	}
-
-	if (_mission_result_sub.updated()) {
-		mission_result_s mission_result{};
-		_mission_result_sub.copy(&mission_result);
-		_mission_finished = mission_result.finished;
 	}
 }
 
@@ -310,9 +296,10 @@ void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius()
 {
 	position_setpoint_triplet_s position_setpoint_triplet{};
 	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+	_curr_wp_type = position_setpoint_triplet.current.type;
 
 	RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
-			_curr_pos_ned, _home_position, _global_ned_proj_ref);
+			_curr_pos_ned, _global_ned_proj_ref);
 
 	_prev_waypoint_transition_angle = _waypoint_transition_angle;
 	_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
@@ -339,13 +326,16 @@ float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transi
 {
 	// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
 	float acceptance_radius = default_acceptance_radius;
-	const float theta = waypoint_transition_angle / 2.f;
+
-	const float min_turning_radius = wheel_base / sinf(max_steer_angle);
+	if (PX4_ISFINITE(_waypoint_transition_angle)) {
-	const float acceptance_radius_temp = min_turning_radius / tanf(theta);
+		const float theta = waypoint_transition_angle / 2.f;
-	const float acceptance_radius_temp_scaled = acceptance_radius_gain *
+		const float min_turning_radius = wheel_base / sinf(max_steer_angle);
-			acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
+		const float acceptance_radius_temp = min_turning_radius / tanf(theta);
-	acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
+		const float acceptance_radius_temp_scaled = acceptance_radius_gain *
-			    acceptance_radius_max);
+				acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
+		acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
+				    acceptance_radius_max);
+	}
 
 	// Publish updated acceptance radius
 	position_controller_status_s pos_ctrl_status{};
@@ -357,7 +347,7 @@ float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transi
 
 float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min,
 		const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad,
-		const float prev_acc_rad, const float max_decel, const float max_jerk, const int nav_state,
+		const float prev_acc_rad, const float max_decel, const float max_jerk, const int curr_wp_type,
 		const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed)
 {
 	// Catch improper values
@@ -365,13 +355,24 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons
 		return cruising_speed;
 	}
 
+	// Upcoming stop
+	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle)
+			|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+			|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
+		const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
+						  max_decel, distance_to_curr_wp, 0.f);
+		return math::min(straight_line_speed, cruising_speed);
+	}
+
 	// Cornering slow down effect
-	if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) {
+	if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) {
 		const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f);
 		const float cornering_speed = _max_yaw_rate * turning_circle;
 		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
 
-	} else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) {
+	}
+
+	if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) {
 		const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
 		const float cornering_speed = _max_yaw_rate * turning_circle;
 		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
@@ -380,26 +381,17 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons
 
 	// Straight line speed
 	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
-		float straight_line_speed{0.f};
+		const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
-
+		float cornering_speed = _max_yaw_rate * turning_circle;
-		if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+		cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed);
-			straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
+		const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-					      max_decel, distance_to_curr_wp, 0.f);
+						  max_decel, distance_to_curr_wp - acc_rad, cornering_speed);
-
-		} else {
-			const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
-			float cornering_speed = _max_yaw_rate * turning_circle;
-			cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed);
-			straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-					      max_decel, distance_to_curr_wp - acc_rad, cornering_speed);
-		}
-
 		return math::min(straight_line_speed, cruising_speed);
 
-	} else {
-		return cruising_speed;
 	}
 
+	return cruising_speed; // Fallthrough
+
 }
 
 bool AckermannPosVelControl::runSanityChecks()

src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp

@@ -58,12 +58,10 @@
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/position_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/position_controller_status.h>
-#include <uORB/topics/mission_result.h>
-#include <uORB/topics/home_position.h>
 #include <uORB/topics/pure_pursuit_status.h>
 
 using namespace matrix;
@@ -125,11 +123,6 @@ private:
 	 */
 	void autoPositionMode();
 
-	/**
-	 * @brief Update uORB subscriptions used for auto modes.
-	 */
-	void updateAutoSubscriptions();
-
 	/**
 	 * @brief Update global/NED waypoint coordinates and acceptance radius.
 	 */
@@ -161,14 +154,14 @@ private:
 	 * @param prev_acc_rad Acceptance radius of the previous waypoint [m].
 	 * @param max_decel Maximum allowed deceleration [m/s^2].
 	 * @param max_jerk Maximum allowed jerk [m/s^3].
-	 * @param nav_state Current nav_state of the rover.
+	 * @param curr_wp_type Type of the current waypoint.
 	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 	 * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 	 * @param max_speed Maximum speed setpoint [m/s]
 	 * @return Speed setpoint [m/s].
 	 */
 	float calcSpeedSetpoint(float cruising_speed, float miss_speed_min, float distance_to_prev_wp,
-				float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int nav_state,
+				float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int curr_wp_type,
 				float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed);
 
 	/**
@@ -185,10 +178,6 @@ private:
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
-	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
 	vehicle_control_mode_s _vehicle_control_mode{};
 	offboard_control_mode_s _offboard_control_mode{};
 
@@ -213,13 +202,11 @@ private:
 	float _speed_body_x_setpoint{0.f};
 	float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
 	float _dt{0.f};
-	int _nav_state{0};
+	int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
 	bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode.
-	bool _mission_finished{false};
 	bool _prev_param_check_passed{true};
 
 	// Waypoint variables
-	Vector2d _home_position{};
 	Vector2f _curr_wp_ned{};
 	Vector2f _prev_wp_ned{};
 	Vector2f _next_wp_ned{};

src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp

@@ -145,12 +145,6 @@ void DifferentialPosVelControl::updateSubscriptions()
 		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
 	}
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-		_vehicle_status_sub.copy(&vehicle_status);
-		_nav_state = vehicle_status.nav_state;
-	}
-
 }
 
 void DifferentialPosVelControl::generateAttitudeSetpoint()
@@ -268,14 +262,32 @@ void DifferentialPosVelControl::offboardVelocityMode()
 
 void DifferentialPosVelControl::autoPositionMode()
 {
-	updateAutoSubscriptions();
+	if (_position_setpoint_triplet_sub.updated()) {
+		position_setpoint_triplet_s position_setpoint_triplet{};
+		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+		_curr_wp_type = position_setpoint_triplet.current.type;
+
+		RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
+				_curr_pos_ned, _global_ned_proj_ref);
+
+		_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
+
+		// Waypoint cruising speed
+		_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
+					  position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
+	}
 
 	// Distances to waypoints
 	const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
 					       2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
 
-	if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival
+	// Check stopping conditions
-		_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
+	bool auto_stop{false};
+
+	if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+	    || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE
+	    || !_next_wp_ned.isAllFinite()) { // Check stopping conditions
+		auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
 	}
 
 	// State machine
@@ -287,7 +299,7 @@ void DifferentialPosVelControl::autoPositionMode()
 	_pure_pursuit_status_pub.publish(pure_pursuit_status);
 	const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw);
 
-	if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) {
+	if (!auto_stop) {
 		if (_currentState == GuidanceState::STOPPED) {
 			_currentState = GuidanceState::DRIVING;
 		}
@@ -309,80 +321,66 @@ void DifferentialPosVelControl::autoPositionMode()
 			// Calculate desired speed in body x direction
 			_speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
 						 _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(),
-						 _param_rd_miss_spd_gain.get());
+						 _param_rd_miss_spd_gain.get(), _curr_wp_type);
+			rover_attitude_setpoint_s rover_attitude_setpoint{};
+			rover_attitude_setpoint.timestamp = _timestamp;
+			rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
+			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 
 		} break;
 
-	case GuidanceState::SPOT_TURNING:
+	case GuidanceState::SPOT_TURNING: {
-		if (fabsf(_vehicle_speed_body_x) > 0.f) {
+			_speed_body_x_setpoint = 0.f;
-			yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop
 
-		}
+			if (fabsf(_vehicle_speed_body_x) > 0.f) {
+				yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop
 
-		_speed_body_x_setpoint = 0.f;
+			}
-		break;
+
+			rover_attitude_setpoint_s rover_attitude_setpoint{};
+			rover_attitude_setpoint.timestamp = _timestamp;
+			rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
+			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+		} break;
 
 	case GuidanceState::STOPPED:
 	default:
-		yaw_setpoint = _vehicle_yaw;
 		_speed_body_x_setpoint = 0.f;
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = _timestamp;
+		rover_rate_setpoint.yaw_rate_setpoint = 0.f;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
 		break;
 
 	}
 
-	rover_attitude_setpoint_s rover_attitude_setpoint{};
-	rover_attitude_setpoint.timestamp = _timestamp;
-	rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
-	_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-}
 
-void DifferentialPosVelControl::updateAutoSubscriptions()
-{
-	if (_home_position_sub.updated()) {
-		home_position_s home_position{};
-		_home_position_sub.copy(&home_position);
-		_home_position = Vector2d(home_position.lat, home_position.lon);
-	}
-
-	if (_position_setpoint_triplet_sub.updated()) {
-		position_setpoint_triplet_s position_setpoint_triplet{};
-		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
-
-		RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
-				_curr_pos_ned, _home_position, _global_ned_proj_ref);
-
-		_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
-
-		// Waypoint cruising speed
-		_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
-					  position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
-	}
-
-	if (_mission_result_sub.updated()) {
-		mission_result_s mission_result{};
-		_mission_result_sub.copy(&mission_result);
-		_mission_finished = mission_result.finished;
-	}
 }
 
 float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp,
 		const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
-		const float trans_drv_trn, const float miss_spd_gain)
+		const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type)
 {
-	float speed_body_x_setpoint = cruising_speed;
+	// Upcoming stop
-
+	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle)
-	if (_waypoint_transition_angle < M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON) {
+			|| _waypoint_transition_angle < M_PI_F - trans_drv_trn || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
-		speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, 0.0f);
+			|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
-
+		const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-	} else if (_waypoint_transition_angle >= M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON
+						  max_decel, distance_to_curr_wp, 0.f);
-		   && miss_spd_gain > FLT_EPSILON) {
+		return math::min(straight_line_speed, cruising_speed);
-		const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle,
-					      0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
-		speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp,
-					max_speed * (1.f - speed_reduction));
 	}
 
-	return math::constrain(speed_body_x_setpoint, -cruising_speed, cruising_speed);
+	// Straight line speed
+	if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) {
+		const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle,
+					      0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
+		const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel,
+						  distance_to_curr_wp,
+						  max_speed * (1.f - speed_reduction));
+		return math::min(straight_line_speed, cruising_speed);
+	}
+
+	return cruising_speed; // Fallthrough
 
 }
 

src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp

@@ -59,12 +59,9 @@
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/position_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/position_controller_status.h>
-#include <uORB/topics/mission_result.h>
-#include <uORB/topics/home_position.h>
 
 using namespace matrix;
 
@@ -134,11 +131,6 @@ private:
 	 */
 	void autoPositionMode();
 
-	/**
-	 * @brief Update uORB subscriptions used for auto modes.
-	 */
-	void updateAutoSubscriptions();
-
 	/**
 	 * @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to
 	 * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
@@ -152,10 +144,11 @@ private:
 	 * @param max_speed Maximum speed setpoint [m/s]
 	 * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad].
 	 * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
+	 * @param curr_wp_type Type of the current waypoint.
 	 * @return Speed setpoint [m/s].
 	 */
 	float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk,
-				float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain);
+				float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain, int curr_wp_type);
 
 
 	/**
@@ -172,10 +165,6 @@ private:
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
-	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
 	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
 	vehicle_control_mode_s _vehicle_control_mode{};
 	offboard_control_mode_s _offboard_control_mode{};
@@ -186,7 +175,6 @@ private:
 	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
 	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
 	uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
-	uORB::Publication<position_controller_status_s>	_position_controller_status_pub{ORB_ID(position_controller_status)};
 	uORB::Publication<pure_pursuit_status_s>	_pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
 
 	// Variables
@@ -201,12 +189,10 @@ private:
 	float _max_yaw_rate{0.f};
 	float _speed_body_x_setpoint{0.f};
 	float _dt{0.f};
-	int _nav_state{0};
+	int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
 	bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode.
-	bool _mission_finished{false};
 
 	// Waypoint variables
-	Vector2d _home_position{};
 	Vector2f _curr_wp_ned{};
 	Vector2f _prev_wp_ned{};
 	Vector2f _next_wp_ned{};

src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp

@@ -165,12 +165,6 @@ void MecanumPosVelControl::updateSubscriptions()
 		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
 	}
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-		_vehicle_status_sub.copy(&vehicle_status);
-		_nav_state = vehicle_status.nav_state;
-	}
-
 }
 
 void MecanumPosVelControl::generateAttitudeSetpoint()
@@ -313,48 +307,13 @@ void MecanumPosVelControl::offboardVelocityMode()
 
 void MecanumPosVelControl::autoPositionMode()
 {
-	updateAutoSubscriptions();
-
-	const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
-					       2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
-
-	if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival
-		_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
-	}
-
-	const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
-					 _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
-					 _nav_state);
-	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(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
-				       velocity_magnitude);
-	_pure_pursuit_status_pub.publish(pure_pursuit_status);
-	const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
-	Vector2f desired_velocity(0.f, 0.f);
-	_speed_body_x_setpoint = _mission_finished ? 0.f : velocity_magnitude * cosf(bearing_setpoint_body_frame);
-	_speed_body_y_setpoint = _mission_finished ? 0.f : velocity_magnitude * sinf(bearing_setpoint_body_frame);
-	rover_attitude_setpoint_s rover_attitude_setpoint{};
-	rover_attitude_setpoint.timestamp = _timestamp;
-	rover_attitude_setpoint.yaw_setpoint = _auto_yaw;
-	_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-}
-
-void MecanumPosVelControl::updateAutoSubscriptions()
-{
-	if (_home_position_sub.updated()) {
-		home_position_s home_position{};
-		_home_position_sub.copy(&home_position);
-		_home_position = Vector2d(home_position.lat, home_position.lon);
-	}
-
 	if (_position_setpoint_triplet_sub.updated()) {
 		position_setpoint_triplet_s position_setpoint_triplet{};
 		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+		_curr_wp_type = position_setpoint_triplet.current.type;
 
 		RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
-				_curr_pos_ned, _home_position, _global_ned_proj_ref);
+				_curr_pos_ned, _global_ned_proj_ref);
 
 		_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
 
@@ -371,38 +330,72 @@ void MecanumPosVelControl::updateAutoSubscriptions()
 		}
 	}
 
-	if (_mission_result_sub.updated()) {
+	const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
-		mission_result_s mission_result{};
+					       2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
-		_mission_result_sub.copy(&mission_result);
+
-		_mission_finished = mission_result.finished;
+	// Check stopping conditions
+	bool auto_stop{false};
+
+	if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+	    || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE
+	    || !_next_wp_ned.isAllFinite()) { // Check stopping conditions
+		auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
+	}
+
+	if (auto_stop) {
+		_speed_body_x_setpoint = 0.f;
+		_speed_body_y_setpoint = 0.f;
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = _timestamp;
+		rover_rate_setpoint.yaw_rate_setpoint = 0.f;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+
+	} else { // Regular guidance algorithm
+		const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
+						 _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
+						 _curr_wp_type);
+		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(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
+					       velocity_magnitude);
+		_pure_pursuit_status_pub.publish(pure_pursuit_status);
+		const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
+		Vector2f desired_velocity(0.f, 0.f);
+		_speed_body_x_setpoint = velocity_magnitude * cosf(bearing_setpoint_body_frame);
+		_speed_body_y_setpoint = velocity_magnitude * sinf(bearing_setpoint_body_frame);
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = _timestamp;
+		rover_attitude_setpoint.yaw_setpoint = _auto_yaw;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 	}
 }
 
 float MecanumPosVelControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp,
 		const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
-		const float miss_spd_gain, const int nav_state)
+		const float miss_spd_gain, const int curr_wp_type)
 {
-	float velocity_magnitude{auto_speed};
+	// Upcoming stop
-
+	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle)
-	if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON
+			|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
-	    && miss_spd_gain > FLT_EPSILON) {
+			|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
-		float max_velocity_magnitude = velocity_magnitude;
+		const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-
+						     max_decel, distance_to_curr_wp, 0.f);
-		if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+		return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
-			max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-						 max_decel, distance_to_curr_wp, 0.f);
-
-		} else if (PX4_ISFINITE(waypoint_transition_angle)) {
-			const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
-						      M_PI_F, 0.f, 1.f), 0.f, 1.f);
-			max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp,
-						 max_speed * (1.f - speed_reduction));
-		}
-
-		velocity_magnitude = math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
 	}
 
-	return velocity_magnitude;
+	// Straight line speed
+	if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) {
+		const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
+					      M_PI_F, 0.f, 1.f), 0.f, 1.f);
+		const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel,
+						     distance_to_curr_wp,
+						     max_speed * (1.f - speed_reduction));
+
+		return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
+	}
+
+	return auto_speed; // Fallthrough
 }
 
 bool MecanumPosVelControl::runSanityChecks()

src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp

@@ -59,12 +59,10 @@
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/position_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/pure_pursuit_status.h>
-#include <uORB/topics/position_controller_status.h>
-#include <uORB/topics/mission_result.h>
-#include <uORB/topics/home_position.h>
 
 using namespace matrix;
 
@@ -125,11 +123,6 @@ private:
 	 */
 	void autoPositionMode();
 
-	/**
-	 * @brief Update uORB subscriptions used for auto modes.
-	 */
-	void updateAutoSubscriptions();
-
 	/**
 	 * @brief Calculate the velocity magnitude setpoint. During waypoint transition the speed is restricted to
 	 * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
@@ -142,11 +135,11 @@ private:
 	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 	 * @param max_speed Maximum velocity magnitude setpoint [m/s]
 	 * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
-	 * @param nav_state Vehicle navigation state
+	 * @param curr_wp_type Type of the current waypoint.
 	 * @return Velocity magnitude setpoint [m/s].
 	 */
 	float calcVelocityMagnitude(float auto_speed, float distance_to_curr_wp, float max_decel, float max_jerk,
-				    float waypoint_transition_angle, float max_speed, float miss_spd_gain, int nav_state);
+				    float waypoint_transition_angle, float max_speed, float miss_spd_gain, int curr_wp_type);
 
 	/**
 	 * @brief Check if the necessary parameters are set.
@@ -162,10 +155,6 @@ private:
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
-	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
 	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
 	vehicle_control_mode_s    _vehicle_control_mode{};
 	offboard_control_mode_s   _offboard_control_mode{};
@@ -176,19 +165,14 @@ private:
 	uORB::Publication<rover_throttle_setpoint_s>    _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
 	uORB::Publication<rover_attitude_setpoint_s>    _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
 	uORB::Publication<rover_velocity_status_s>      _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
-	uORB::Publication<position_controller_status_s>	_position_controller_status_pub{ORB_ID(position_controller_status)};
 	uORB::Publication<pure_pursuit_status_s>	_pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
 
 	// Variables
 	hrt_abstime _timestamp{0};
 	Quatf _vehicle_attitude_quaternion{};
-	Vector2d _home_position{};
 	Vector2f _curr_pos_ned{};
 	Vector2f _pos_ctl_course_direction{};
 	Vector2f _pos_ctl_start_position_ned{};
-	Vector2f _curr_wp_ned{};
-	Vector2f _prev_wp_ned{};
-	Vector2f _next_wp_ned{};
 	float _vehicle_speed_body_x{0.f};
 	float _vehicle_speed_body_y{0.f};
 	float _vehicle_yaw{0.f};
@@ -199,11 +183,15 @@ private:
 	float _dt{0.f};
 	float _auto_speed{0.f};
 	float _auto_yaw{0.f};
-	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
-	int _nav_state{0};
-	bool _mission_finished{false};
 	bool _prev_param_check_passed{true};
 
+	// Waypoint variables
+	Vector2f _curr_wp_ned{};
+	Vector2f _prev_wp_ned{};
+	Vector2f _next_wp_ned{};
+	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+
 	// Controllers
 	PID _pid_speed_x;
 	PID _pid_speed_y;