ackermann: update position control

msg/RoverPositionSetpoint.msg

@@ -1,6 +1,8 @@
 uint64 timestamp # time since system start (microseconds)
 
 float32[2] position_ned # 2-dimensional position setpoint in NED frame [m]
-float32 cruising_speed  # (Optional) Specify rover speed (Defaults to maximum speed)
+float32[2] start_ned	# (Optional) 2-dimensional start position in NED frame used to define the line that the rover will track to position_ned [m] (Defaults to vehicle position)
+float32 cruising_speed  # (Optional) Specify rover speed [m/s] (Defaults to maximum speed)
+float32 arrival_speed   # (Optional) Specify arrival speed [m/s] (Defaults to zero)
 
-float32 yaw             # [rad] [-pi,pi] from North. Optional, NAN if not set. Mecanum only.
+float32 yaw             # [rad] [-pi,pi] from North. Optional, NAN if not set. Mecanum only. (Defaults to vehicle yaw)

src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp

@@ -41,10 +41,6 @@ AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(pa
 	_rover_position_setpoint_pub.advertise();
 	_rover_velocity_setpoint_pub.advertise();
 
-	// Initially set to NaN to indicate that the rover has no position setpoint
-	_rover_position_setpoint.position_ned[0] = NAN;
-	_rover_position_setpoint.position_ned[1] = NAN;
-
 	updateParams();
 }
 
@@ -61,21 +57,24 @@ void AckermannPosControl::updateParams()
 
 void AckermannPosControl::updatePosControl()
 {
-	const hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
 	updateSubscriptions();
 
 	if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
+		// Generate Position Setpoint
 		if (_vehicle_control_mode.flag_control_offboard_enabled) {
 			generatePositionSetpoint();
+
+		} else if (_vehicle_control_mode.flag_control_manual_enabled) {
+			manualPositionMode();
+
+		} else if (_vehicle_control_mode.flag_control_auto_enabled) {
+			autoPositionMode();
 		}
 
+		// Generate Velocity Setpoint
 		generateVelocitySetpoint();
 
 	}
-
 }
 
 void AckermannPosControl::updateSubscriptions()
@@ -121,7 +120,7 @@ void AckermannPosControl::generatePositionSetpoint()
 
 	// Translate trajectory setpoint to rover position setpoint
 	rover_position_setpoint_s rover_position_setpoint{};
-	rover_position_setpoint.timestamp = _timestamp;
+	rover_position_setpoint.timestamp = hrt_absolute_time();
 	rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
 	rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
 	rover_position_setpoint.cruising_speed = _param_ro_speed_limit.get();
@@ -130,31 +129,10 @@ void AckermannPosControl::generatePositionSetpoint()
 
 }
 
-void AckermannPosControl::generateVelocitySetpoint()
-{
-	// Manual Position Mode
-	if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) {
-		manualPositionMode();
-		return;
-	}
-
-	// Auto Mode
-	if (_vehicle_control_mode.flag_control_auto_enabled) {
-		autoPositionMode();
-		return;
-	}
-
-	// Rover Position Setpoint
-	if (_rover_position_setpoint_sub.copy(&_rover_position_setpoint)
-	    && PX4_ISFINITE(_rover_position_setpoint.position_ned[0]) && PX4_ISFINITE(_rover_position_setpoint.position_ned[1])) {
-		goToPositionMode();
-		return;
-	}
-
-}
-
 void AckermannPosControl::manualPositionMode()
 {
+	updateSubscriptions();
+
 	manual_control_setpoint_s manual_control_setpoint{};
 	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
 
@@ -167,12 +145,21 @@ void AckermannPosControl::manualPositionMode()
 	if (fabsf(yaw_delta) > FLT_EPSILON
 	    || fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
 		_course_control = false;
+		// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
 		const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta);
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = _timestamp;
-		rover_velocity_setpoint.speed = speed_setpoint;
-		rover_velocity_setpoint.bearing = yaw_setpoint;
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+		const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint));
+		const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _param_pp_lookahd_max.get() *
+						     pos_ctl_course_direction;
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
+		rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
+		rover_position_setpoint.start_ned[0] = NAN;
+		rover_position_setpoint.start_ned[1] = NAN;
+		rover_position_setpoint.arrival_speed = NAN;
+		rover_position_setpoint.cruising_speed = speed_setpoint;
+		rover_position_setpoint.yaw = NAN;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
 
 	} else { // Course control if the steering input is zero (keep driving on a straight line)
 		if (!_course_control) {
@@ -186,24 +173,23 @@ void AckermannPosControl::manualPositionMode()
 		const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
 		const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) *
 						     vector_scaling * _pos_ctl_course_direction;
-		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, _pos_ctl_start_position_ned,
-					       _curr_pos_ned, fabsf(speed_setpoint));
-		_pure_pursuit_status_pub.publish(pure_pursuit_status);
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = _timestamp;
-		rover_velocity_setpoint.speed = speed_setpoint;
-		rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
-				bearing_setpoint + M_PI_F);
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
-
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
+		rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
+		rover_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0);
+		rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1);
+		rover_position_setpoint.arrival_speed = NAN;
+		rover_position_setpoint.cruising_speed = speed_setpoint;
+		rover_position_setpoint.yaw = NAN;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
 	}
 }
 
 void AckermannPosControl::autoPositionMode()
 {
+	updateSubscriptions();
+
 	if (_position_setpoint_triplet_sub.updated()) {
 		updateWaypointsAndAcceptanceRadius();
 	}
@@ -214,40 +200,19 @@ void AckermannPosControl::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));
 
-	// 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()) {
-		auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
-	}
-
-	if (auto_stop) {
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = _timestamp;
-		rover_velocity_setpoint.speed = 0.f;
-		rover_velocity_setpoint.bearing = _vehicle_yaw;
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
-
-	} else { // Regular guidance algorithm
-		const float speed_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(), _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;
-		const float yaw_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,
-					   fabsf(speed_setpoint));
-		_pure_pursuit_status_pub.publish(pure_pursuit_status);
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = _timestamp;
-		rover_velocity_setpoint.speed = speed_setpoint;
-		rover_velocity_setpoint.bearing = yaw_setpoint;
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
-
-	}
+	rover_position_setpoint_s rover_position_setpoint{};
+	rover_position_setpoint.timestamp = hrt_absolute_time();
+	rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
+	rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
+	rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
+	rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
+	rover_position_setpoint.arrival_speed = autoArrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type,
+						_waypoint_transition_angle, _max_yaw_rate);
+	rover_position_setpoint.cruising_speed = autoCruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp,
+			distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle,
+			_prev_waypoint_transition_angle, _max_yaw_rate);
+	rover_position_setpoint.yaw = NAN;
+	_rover_position_setpoint_pub.publish(rover_position_setpoint);
 }
 
 void AckermannPosControl::updateWaypointsAndAcceptanceRadius()
@@ -298,87 +263,104 @@ float AckermannPosControl::updateAcceptanceRadius(const float waypoint_transitio
 	// Publish updated acceptance radius
 	position_controller_status_s pos_ctrl_status{};
 	pos_ctrl_status.acceptance_radius = acceptance_radius;
-	pos_ctrl_status.timestamp = _timestamp;
+	pos_ctrl_status.timestamp = hrt_absolute_time();
 	_position_controller_status_pub.publish(pos_ctrl_status);
 	return acceptance_radius;
 }
 
-float AckermannPosControl::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 curr_wp_type,
-		const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed)
+float AckermannPosControl::autoArrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad,
+		const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate)
+{
+	if (!PX4_ISFINITE(waypoint_transition_angle)
+	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		return 0.f; // Stop at the waypoint
+
+	} else {
+		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); // Slow down for cornering
+	}
+}
+
+float AckermannPosControl::autoCruisingSpeed(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 waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate)
 {
 	// Catch improper values
 	if (miss_speed_min < -FLT_EPSILON  || miss_speed_min > cruising_speed) {
 		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 && 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;
+		const float cornering_speed = max_yaw_rate * turning_circle;
 		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
 
 	}
 
 	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;
+		const float cornering_speed = max_yaw_rate * turning_circle;
 		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
 
 	}
 
-	// Straight line speed
-	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
-		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);
-		const float 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);
-
-	}
-
 	return cruising_speed; // Fallthrough
 
 }
 
-void AckermannPosControl::goToPositionMode()
+void AckermannPosControl::generateVelocitySetpoint()
 {
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	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]);
+		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
+	}
+
+	if (_position_controller_status_sub.updated()) {
+		position_controller_status_s position_controller_status{};
+		_position_controller_status_sub.copy(&position_controller_status);
+		_acceptance_radius = position_controller_status.acceptance_radius;
+	}
+
 	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
 	const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
 
 	if (distance_to_target > _param_nav_acc_rad.get()) {
+
+		float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
+				      0.f;
+		const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target;
 		float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-				       _param_ro_decel_limit.get(), distance_to_target, 0.f);
-		const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ?
-					_rover_position_setpoint.cruising_speed :
-					_param_ro_speed_limit.get();
-		speed_setpoint = math::min(speed_setpoint, max_speed);
+				       _param_ro_decel_limit.get(), distance, 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));
+		}
+
 		pure_pursuit_status_s pure_pursuit_status{};
-		pure_pursuit_status.timestamp = _timestamp;
+		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, _curr_pos_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);
 		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = _timestamp;
+		rover_velocity_setpoint.timestamp = timestamp;
 		rover_velocity_setpoint.speed = speed_setpoint;
-		rover_velocity_setpoint.bearing = yaw_setpoint;
+		rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(
+				yaw_setpoint + M_PI_F);
 		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
 
 	} else {
 		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = _timestamp;
+		rover_velocity_setpoint.timestamp = timestamp;
 		rover_velocity_setpoint.speed = 0.f;
 		rover_velocity_setpoint.bearing = _vehicle_yaw;
 		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);

src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp

@@ -78,10 +78,20 @@ public:
 	~AckermannPosControl() = default;
 
 	/**
-	 * @brief Update position controller.
+	 * @brief Update position control
 	 */
 	void updatePosControl();
 
+	/**
+	 * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
+	 */
+	void manualPositionMode();
+
+	/**
+	 * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet.
+	 */
+	void autoPositionMode();
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -99,27 +109,6 @@ private:
 	 */
 	void generatePositionSetpoint();
 
-	/**
-	 * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint (Position Mode) or
-	 * 	  positionSetpointTriplet (Auto Mode) or roverPositionSetpoint.
-	 */
-	void generateVelocitySetpoint();
-
-	/**
-	 * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
-	 */
-	void manualPositionMode();
-
-	/**
-	 * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet.
-	 */
-	void autoPositionMode();
-
-	/**
-	 * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint.
-	 */
-	void goToPositionMode();
-
 	/**
 	 * @brief Update global/NED waypoint coordinates and acceptance radius.
 	 */
@@ -140,26 +129,34 @@ private:
 				     float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle);
 
 	/**
-	 * @brief Calculate the speed setpoint. During cornering the speed is restricted based on the radius of the corner.
-	 * On straight lines it is based on a speed trajectory such that the rover will arrive at the next corner with the
-	 * desired cornering speed under consideration of the maximum deceleration and jerk.
+	 * @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner.
 	 * @param cruising_speed Cruising speed [m/s].
 	 * @param miss_speed_min Minimum speed setpoint [m/s].
-	 * @param distance_to_prev_wp Distance to the previous waypoint [m].
-	 * @param distance_to_curr_wp Distance to the current waypoint [m].
 	 * @param acc_rad Acceptance radius of the current waypoint [m].
-	 * @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 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]
+	 * @param max_yaw_rate Maximum yaw rate setpoint [rad/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 curr_wp_type,
-				float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed);
+	float autoArrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type,
+			       float waypoint_transition_angle, float max_yaw_rate);
+
+	/**
+	* @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner.
+	* @param cruising_speed Cruising speed [m/s].
+	* @param miss_speed_min Minimum speed setpoint [m/s].
+	* @param distance_to_prev_wp Distance to the previous waypoint [m].
+	* @param distance_to_curr_wp Distance to the current waypoint [m].
+	* @param acc_rad Acceptance radius of the current waypoint [m].
+	* @param prev_acc_rad Acceptance radius of the previous waypoint [m].
+	* @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_yaw_rate Maximum yaw rate setpoint [rad/s]
+	* @return Speed setpoint [m/s].
+	*/
+	float autoCruisingSpeed(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 waypoint_transition_angle,
+				float prev_waypoint_transition_angle, float max_yaw_rate);
 
 	/**
 	 * @brief Check if the necessary parameters are set.
@@ -167,6 +164,11 @@ private:
 	 */
 	bool runSanityChecks();
 
+	/**
+	 * @brief Generate RoverVelocitySetpoint from RoverPositionSetpoint
+	 */
+	void generateVelocitySetpoint();
+
 	// uORB subscriptions
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
@@ -176,9 +178,10 @@ private:
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
 	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{};
 	vehicle_control_mode_s _vehicle_control_mode{};
 	offboard_control_mode_s _offboard_control_mode{};
-	rover_position_setpoint_s _rover_position_setpoint{};
 
 	// uORB publications
 	uORB::Publication<rover_velocity_setpoint_s>    _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
@@ -187,15 +190,14 @@ private:
 	uORB::Publication<rover_position_setpoint_s>	_rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
 
 	// Variables
-	hrt_abstime _timestamp{0};
 	Quatf _vehicle_attitude_quaternion{};
 	Vector2f _curr_pos_ned{};
 	Vector2f _pos_ctl_course_direction{};
 	Vector2f _pos_ctl_start_position_ned{};
+	Vector2f _start_ned{};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{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 _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 _prev_param_check_passed{true};