rover: clean up speed terminology

msg/RoverThrottleSetpoint.msg

@@ -2,5 +2,4 @@
 uint64 timestamp        # time since system start (microseconds)
 
 float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] (Positiv = forwards, Negativ = backwards)
-
 float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1] (Mecanum only, Positiv = right, Negativ = left)

msg/RoverVelocityStatus.msg

@@ -1,10 +1,8 @@
 uint64 timestamp # time since system start (microseconds)
 
 float32 measured_speed_body_x          # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
-float32 speed_body_x_setpoint          # [m/s] Speed setpoint in body x direction. Positiv: forwards, Negativ: backwards
 float32 adjusted_speed_body_x_setpoint # [m/s] Post slew rate speed setpoint in body x direction. Positiv: forwards, Negativ: backwards
-float32 measured_speed_body_y          # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
-float32 speed_body_y_setpoint          # [m/s] Speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers)
-float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers)
 float32 pid_throttle_body_x_integral   # Integral of the PID for the closed loop controller of the speed in body x direction
-float32 pid_throttle_body_y_integral   # Integral of the PID for the closed loop controller of the speed in body y direction
+float32 measured_speed_body_y          # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left (Mecanum only)
+float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Mecanum only)
+float32 pid_throttle_body_y_integral   # Integral of the PID for the closed loop controller of the speed in body y direction (Mecanum only)

src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp

@@ -158,21 +158,21 @@ void AckermannPosControl::manualPositionMode()
 	manual_control_setpoint_s manual_control_setpoint{};
 	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
 
-	const float speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+	const float speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-					    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
+				     -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
 	const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
 				_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
 				_max_yaw_rate / _param_ro_yaw_p.get());
 
 	if (fabsf(yaw_delta) > FLT_EPSILON
-	    || fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
+	    || fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
 		_course_control = false;
 		const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
 		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
 		ackermann_velocity_setpoint.timestamp = _timestamp;
-		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
-		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
-		ackermann_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON;
+		ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON;
 		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
 
 	} else { // Course control if the steering input is zero (keep driving on a straight line)
@@ -185,19 +185,19 @@ void AckermannPosControl::manualPositionMode()
 		// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
 		const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
 		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_body_x_setpoint) *
+		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 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, _pos_ctl_start_position_ned,
-					   _curr_pos_ned, fabsf(speed_body_x_setpoint));
+					   _curr_pos_ned, fabsf(speed_setpoint));
 		_pure_pursuit_status_pub.publish(pure_pursuit_status);
 		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
 		ackermann_velocity_setpoint.timestamp = _timestamp;
-		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
-		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
-		ackermann_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON;
+		ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON;
 		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
 	}
 }
@@ -232,20 +232,20 @@ void AckermannPosControl::autoPositionMode()
 		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
 
 	} else { // Regular guidance algorithm
-		const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp,
+		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(),
+					     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_jerk_limit.get(), _curr_wp_type, _waypoint_transition_angle, _prev_waypoint_transition_angle,
-						    _param_ro_speed_limit.get());
+					     _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_body_x_setpoint));
+					   fabsf(speed_setpoint));
 		_pure_pursuit_status_pub.publish(pure_pursuit_status);
 		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
 		ackermann_velocity_setpoint.timestamp = _timestamp;
-		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
-		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
 		ackermann_velocity_setpoint.backwards = false;
 		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
 
@@ -360,22 +360,22 @@ void AckermannPosControl::goToPositionMode()
 	const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
 
 	if (distance_to_target > _param_nav_acc_rad.get()) {
-		const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
+		float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-					     _param_ro_decel_limit.get(), distance_to_target, 0.f);
+				       _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();
-		const float speed_body_x_setpoint = math::min(speed_setpoint, max_speed);
+		speed_setpoint = math::min(speed_setpoint, max_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, _curr_pos_ned,
-					   _curr_pos_ned, fabsf(speed_body_x_setpoint));
+					   _curr_pos_ned, fabsf(speed_setpoint));
 		_pure_pursuit_status_pub.publish(pure_pursuit_status);
 		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
 		ackermann_velocity_setpoint.timestamp = _timestamp;
-		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
-		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
 		ackermann_velocity_setpoint.backwards = false;
 		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
 

src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp

@@ -80,11 +80,11 @@ void AckermannVelControl::updateVelControl()
 	// Publish position controller status (logging only)
 	rover_velocity_status_s rover_velocity_status;
 	rover_velocity_status.timestamp = _timestamp;
-	rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x;
+	rover_velocity_status.measured_speed_body_x = _vehicle_speed;
 	rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState();
-	rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
-	rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
 	rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
+	rover_velocity_status.measured_speed_body_y = NAN;
+	rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
 	rover_velocity_status.pid_throttle_body_y_integral = NAN;
 	_rover_velocity_status_pub.publish(rover_velocity_status);
 }
@@ -106,10 +106,10 @@ void AckermannVelControl::updateSubscriptions()
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
 
-		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
-		_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
+		Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1));
-		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
+		_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
 	}
 
 }
@@ -143,9 +143,16 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
 		_ackermann_velocity_setpoint_sub.copy(&_ackermann_velocity_setpoint);
 	}
 
+	const Vector2f velocity_ned = Vector2f(_ackermann_velocity_setpoint.velocity_ned[0],
+					       _ackermann_velocity_setpoint.velocity_ned[1]);
+
+	// Santitize input
+	if (!velocity_ned.isAllFinite()) {
+		return;
+	}
+
 	// Attitude Setpoint
-	if (fabsf(_ackermann_velocity_setpoint.velocity_ned[1]) < FLT_EPSILON
+	if (velocity_ned.norm() < FLT_EPSILON) {
-	    && fabsf(_ackermann_velocity_setpoint.velocity_ned[0]) < FLT_EPSILON) {
 		rover_attitude_setpoint_s rover_attitude_setpoint{};
 		rover_attitude_setpoint.timestamp = _timestamp;
 		rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
@@ -154,23 +161,21 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
 	} else {
 		rover_attitude_setpoint_s rover_attitude_setpoint{};
 		rover_attitude_setpoint.timestamp = _timestamp;
-		const float yaw_setpoint = atan2f(_ackermann_velocity_setpoint.velocity_ned[1],
+		const float yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
-						  _ackermann_velocity_setpoint.velocity_ned[0]);
 		rover_attitude_setpoint.yaw_setpoint = _ackermann_velocity_setpoint.backwards ? matrix::wrap_pi(
 				yaw_setpoint + M_PI_F) : yaw_setpoint;
 		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 	}
 
 	// Throttle Setpoint
-	const float speed_magnitude = math::min(sqrtf(powf(_ackermann_velocity_setpoint.velocity_ned[0],
+	const float speed_magnitude = math::min(velocity_ned.norm(), _param_ro_speed_limit.get());
-						2) + powf(_ackermann_velocity_setpoint.velocity_ned[1], 2)), _param_ro_speed_limit.get());
+	const float speed_setpoint = _ackermann_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude;
-	const float speed_body_x_setpoint = _ackermann_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude;
 	rover_throttle_setpoint_s rover_throttle_setpoint{};
 	rover_throttle_setpoint.timestamp = _timestamp;
 	rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed,
-			speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
+			speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
 			_param_ro_max_thr_speed.get(), _dt);
-	rover_throttle_setpoint.throttle_body_y = 0.f;
+	rover_throttle_setpoint.throttle_body_y = NAN;
 	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
 
 }

src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp

@@ -126,8 +126,7 @@ private:
 	// Variables
 	hrt_abstime _timestamp{0};
 	Quatf _vehicle_attitude_quaternion{};
-	float _vehicle_speed_body_x{0.f};
+	float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
-	float _vehicle_speed_body_y{0.f};
 	float _vehicle_yaw{0.f};
 	float _dt{0.f};
 	bool _prev_param_check_passed{true};

src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp

@@ -97,9 +97,10 @@ void DifferentialPosControl::updateSubscriptions()
 		}
 
 		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
-		const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-		const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
-		_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
+		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;
 	}
 
 }
@@ -148,14 +149,14 @@ void DifferentialPosControl::manualPositionMode()
 	manual_control_setpoint_s manual_control_setpoint{};
 	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
 
-	const float speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+	const float speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-					    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
+				     -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
 	const float bearing_scaling = math::min(_max_yaw_rate / _param_ro_yaw_p.get(),
 						_param_rd_trans_drv_trn.get() - FLT_EPSILON);
 	const float bearing_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
 				    _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -bearing_scaling, bearing_scaling);
 
-	if (fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Turn on spot
+	if (fabsf(speed_setpoint) < FLT_EPSILON) { // Turn on spot
 		_course_control = false;
 		const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta);
 		differential_velocity_setpoint_s differential_velocity_setpoint{};
@@ -169,7 +170,7 @@ void DifferentialPosControl::manualPositionMode()
 		const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta);
 		differential_velocity_setpoint_s differential_velocity_setpoint{};
 		differential_velocity_setpoint.timestamp = _timestamp;
-		differential_velocity_setpoint.speed = speed_body_x_setpoint;
+		differential_velocity_setpoint.speed = speed_setpoint;
 		differential_velocity_setpoint.bearing = bearing_setpoint;
 		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
 
@@ -183,18 +184,18 @@ void DifferentialPosControl::manualPositionMode()
 		// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
 		const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
 		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_body_x_setpoint) *
+		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_body_x_setpoint));
+					       _curr_pos_ned, fabsf(speed_setpoint));
 		_pure_pursuit_status_pub.publish(pure_pursuit_status);
 		differential_velocity_setpoint_s differential_velocity_setpoint{};
 		differential_velocity_setpoint.timestamp = _timestamp;
-		differential_velocity_setpoint.speed = speed_body_x_setpoint;
+		differential_velocity_setpoint.speed = speed_setpoint;
-		differential_velocity_setpoint.bearing = speed_body_x_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
+		differential_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
 					bearing_setpoint + M_PI_F);
 		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
 	}
@@ -238,18 +239,18 @@ void DifferentialPosControl::autoPositionMode()
 		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
 
 	} else {
-		const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
+		const float speed_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_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(),
-						    _param_rd_miss_spd_gain.get(), _curr_wp_type);
+					     _param_rd_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,
-					       fabsf(speed_body_x_setpoint));
+					       fabsf(speed_setpoint));
 		_pure_pursuit_status_pub.publish(pure_pursuit_status);
 		differential_velocity_setpoint_s differential_velocity_setpoint{};
 		differential_velocity_setpoint.timestamp = _timestamp;
-		differential_velocity_setpoint.speed = speed_body_x_setpoint;
+		differential_velocity_setpoint.speed = speed_setpoint;
 		differential_velocity_setpoint.bearing = bearing_setpoint;
 		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
 	}
@@ -289,21 +290,21 @@ void DifferentialPosControl::goToPositionMode()
 	const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
 
 	if (distance_to_target > _param_nav_acc_rad.get()) {
-		const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
+		float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-					     _param_ro_decel_limit.get(), distance_to_target, 0.f);
+				       _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();
-		const float speed_body_x_setpoint = math::min(speed_setpoint, max_speed);
+		speed_setpoint = math::min(speed_setpoint, max_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, _curr_pos_ned,
-					       _curr_pos_ned, fabsf(speed_body_x_setpoint));
+					       _curr_pos_ned, fabsf(speed_setpoint));
 		_pure_pursuit_status_pub.publish(pure_pursuit_status);
 		differential_velocity_setpoint_s differential_velocity_setpoint{};
 		differential_velocity_setpoint.timestamp = _timestamp;
-		differential_velocity_setpoint.speed = speed_body_x_setpoint;
+		differential_velocity_setpoint.speed = speed_setpoint;
 		differential_velocity_setpoint.bearing = bearing_setpoint;
 		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
 

src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp

@@ -163,7 +163,7 @@ private:
 	uORB::Publication<rover_velocity_status_s>          _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
 	uORB::Publication<differential_velocity_setpoint_s> _differential_velocity_setpoint_pub{ORB_ID(differential_velocity_setpoint)};
 	uORB::Publication<pure_pursuit_status_s>	    _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
-	uORB::Publication<rover_position_setpoint_s>	 _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
+	uORB::Publication<rover_position_setpoint_s>	    _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
 
 	// Variables
 	hrt_abstime _timestamp{0};
@@ -171,7 +171,7 @@ private:
 	Vector2f _curr_pos_ned{};
 	Vector2f _pos_ctl_course_direction{};
 	Vector2f _pos_ctl_start_position_ned{};
-	float _vehicle_speed_body_x{0.f};
+	float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
 	float _dt{0.f};

src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp

@@ -76,14 +76,14 @@ void DifferentialVelControl::updateVelControl()
 		_speed_setpoint.setForcedValue(0.f);
 	}
 
-	// Publish position controller status (logging only)
+	// Publish velocity controller status (logging only)
 	rover_velocity_status_s rover_velocity_status;
 	rover_velocity_status.timestamp = _timestamp;
-	rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x;
+	rover_velocity_status.measured_speed_body_x = _vehicle_speed;
 	rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState();
-	rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
-	rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
 	rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
+	rover_velocity_status.measured_speed_body_y = NAN;
+	rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
 	rover_velocity_status.pid_throttle_body_y_integral = NAN;
 	_rover_velocity_status_pub.publish(rover_velocity_status);
 }
@@ -103,10 +103,10 @@ void DifferentialVelControl::updateSubscriptions()
 	if (_vehicle_local_position_sub.updated()) {
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
-		const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-		const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
-		_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
+		Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1));
-		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
+		_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
 	}
 
 }
@@ -155,31 +155,31 @@ void DifferentialVelControl::generateAttitudeAndThrottleSetpoint()
 		_current_state = DrivingState::DRIVING;
 	}
 
-	float speed_body_x_setpoint = 0.f;
+	float speed_setpoint = 0.f;
 
 	if (_current_state == DrivingState::DRIVING) {
-		speed_body_x_setpoint = math::constrain(_differential_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
+		speed_setpoint = math::constrain(_differential_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
-							_param_ro_speed_limit.get());
+						 _param_ro_speed_limit.get());
 
-		const float speed_body_x_setpoint_normalized = math::interpolate<float>(speed_body_x_setpoint,
+		const float speed_setpoint_normalized = math::interpolate<float>(speed_setpoint,
-				-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
+							-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
 
 		if (_rover_steering_setpoint_sub.updated()) {
 			_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
 		}
 
-		if (fabsf(speed_body_x_setpoint_normalized) > 1.f - fabsf(
+		if (fabsf(speed_setpoint_normalized) > 1.f - fabsf(
 			    _rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels
-			speed_body_x_setpoint = math::interpolate<float>(sign(speed_body_x_setpoint_normalized) * (1.f - fabsf(
+			speed_setpoint = math::interpolate<float>(sign(speed_setpoint_normalized) * (1.f - fabsf(
-							_rover_steering_setpoint.normalized_speed_diff)), -1.f, 1.f,
+						 _rover_steering_setpoint.normalized_speed_diff)), -1.f, 1.f,
-						- _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
+					 - _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
 		}
 	}
 
 	rover_throttle_setpoint_s rover_throttle_setpoint{};
 	rover_throttle_setpoint.timestamp = _timestamp;
 	rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed,
-			speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
+			speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
 			_param_ro_max_thr_speed.get(), _dt);
 	rover_throttle_setpoint.throttle_body_y = 0.f;
 	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);

src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp

@@ -137,8 +137,7 @@ private:
 	// Variables
 	hrt_abstime _timestamp{0};
 	Quatf _vehicle_attitude_quaternion{};
-	float _vehicle_speed_body_x{0.f};
+	float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
-	float _vehicle_speed_body_y{0.f};
 	float _vehicle_yaw{0.f};
 	float _dt{0.f};
 	bool _prev_param_check_passed{false};