mecanum: closed loop velocity control in mission mode

msg/RoverMecanumGuidanceStatus.msg

@@ -1,6 +1,7 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 desired_speed 	      # [m/s] Desired forward speed for the rover
+float32 desired_forward_speed 	      # [m/s] Desired forward speed for the rover
+float32 desired_lateral_speed 	      # [m/s] Desired forward speed for the rover
 float32 lookahead_distance    # [m] Lookahead distance of pure the pursuit controller
 float32 heading_error_deg 	      # [deg] Heading error of the pure pursuit controller
 float32 pid_heading_integral  # Integral of the PID for the desired yaw rate during missions

msg/RoverMecanumStatus.msg

@@ -1,6 +1,7 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 actual_speed	       # [m/s] Actual forward speed of the rover
+float32 forward_speed	       # [m/s] Actual forward speed of the rover
+float32 lateral_speed	       # [m/s] Actual lateral speed of the rover
 float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate
 float32 actual_yaw_rate_deg_s  # [deg/s] Actual yaw rate of the rover
 float32 pid_yaw_rate_integral  # Integral of the PID for the desired yaw rate controller

src/modules/rover_mecanum/RoverMecanum.cpp

@@ -111,6 +111,7 @@ void RoverMecanum::Run()
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 		_mecanum_setpoint = _rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed,
+				    _vehicle_lateral_speed,
 				    _nav_state);
 		break;
 
@@ -146,7 +147,8 @@ void RoverMecanum::Run()
 	// Publish rover mecanum status (logging)
 	rover_mecanum_status_s rover_mecanum_status{};
 	rover_mecanum_status.timestamp = _timestamp;
-	rover_mecanum_status.actual_speed = _vehicle_forward_speed;
+	rover_mecanum_status.forward_speed = _vehicle_forward_speed;
+	rover_mecanum_status.lateral_speed = _vehicle_lateral_speed;
 	rover_mecanum_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _mecanum_setpoint.yaw_rate;
 	rover_mecanum_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate;
 	rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
@@ -195,6 +197,7 @@ void RoverMecanum::updateSubscriptions()
 		Vector3f velocity_in_local_frame(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);
 		_vehicle_forward_speed = velocity_in_body_frame(0);
+		_vehicle_lateral_speed = velocity_in_body_frame(1);
 	}
 }
 
@@ -209,23 +212,31 @@ matrix::Vector4f RoverMecanum::computeMotorCommands(float forward_speed, float l
 		combined_speed = 1.f;
 	}
 
-	// // Prioritize yaw rate over forward and lateral speed
-	// const float total_speed = combined_speed + fabsf(speed_diff);
-
-	// if (total_speed > 1.f) {
-	// 	const float excess_velocity = fabsf(total_speed - 1.f);
-	// 	forward_speed -= sign(forward_speed) * 0.5f * excess_velocity;
-	// 	lateral_speed -= sign(lateral_speed) * 0.5f * excess_velocity;
-	// }
-
-	// Prioritize lateral movement over yaw rate
+	// Prioritize yaw rate over forward and lateral speed
 	const float total_speed = combined_speed + fabsf(speed_diff);
 
 	if (total_speed > 1.f) {
 		const float excess_velocity = fabsf(total_speed - 1.f);
-		speed_diff -= sign(speed_diff) * excess_velocity;
+		const float forward_speed_temp = forward_speed - sign(forward_speed) * 0.5f * excess_velocity;
+		const float lateral_speed_temp = lateral_speed - sign(lateral_speed) * 0.5f * excess_velocity;
+
+		if (sign(forward_speed_temp) == sign(forward_speed) && sign(lateral_speed) == sign(lateral_speed_temp)) {
+			forward_speed = forward_speed_temp;
+			lateral_speed = lateral_speed_temp;
+
+		} else {
+			forward_speed = lateral_speed = 0.f;
+		}
 	}
 
+	// // Prioritize lateral movement over yaw rate
+	// const float total_speed = combined_speed + fabsf(speed_diff);
+
+	// if (total_speed > 1.f) {
+	// 	const float excess_velocity = fabsf(total_speed - 1.f);
+	// 	speed_diff -= sign(speed_diff) * excess_velocity;
+	// }
+
 	// Calculate motor commands
 	const float input_data[3] = {forward_speed, lateral_speed, speed_diff};
 	const Matrix<float, 3, 1> input(input_data);

src/modules/rover_mecanum/RoverMecanum.hpp

@@ -119,6 +119,7 @@ class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
 	// Variables
 	float _vehicle_body_yaw_rate{0.f};
 	float _vehicle_forward_speed{0.f};
+	float _vehicle_lateral_speed{0.f};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
 	int _nav_state{0};
@@ -128,7 +129,7 @@ class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
 	RoverMecanumGuidance::mecanum_setpoint _mecanum_setpoint;
 
 	// Constants
-	static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control
+	static constexpr float YAW_RATE_ERROR_THRESHOLD = 1.f; // [rad/s] Error threshold for the closed loop yaw rate control
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,

src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp

@@ -44,7 +44,7 @@ RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(
 	_rover_mecanum_guidance_status_pub.advertise();
 	pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f);
 	pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
-
+	pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
 }
 
 void RoverMecanumGuidance::updateParams()
@@ -64,20 +64,25 @@ void RoverMecanumGuidance::updateParams()
 			   0.f, // Derivative gain
 			   1.f, // Integral limit
 			   1.f); // Output limit
+	pid_set_parameters(&_pid_lateral_throttle,
+			   _param_rm_p_gain_speed.get(), // Proportional gain
+			   _param_rm_i_gain_speed.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
 }
 
 RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(const float yaw,
-		const float actual_speed, const int nav_state)
+		const float forward_speed, const float lateral_speed, const int nav_state)
 {
 	// Initializations
 	bool mission_finished{false};
 	float desired_speed{0.f};
-	// float desired_yaw_rate{0.f};
+	float desired_yaw{0.f}; // TODO: Make this a parameter
 	Vector2f desired_velocity(0.f, 0.f);
-	// float lateral_throttle{0.f};
-	// hrt_abstime timestamp_prev = _timestamp;
-	// _timestamp = hrt_absolute_time();
-	// const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
 	// uORB subscriber updates
 	if (_vehicle_global_position_sub.updated()) {
@@ -120,7 +125,7 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con
 					  _curr_wp(0),
 					  _curr_wp(1));
 
-	if (_theta < 2.61799f) {
+	if (_theta < SLOW_DOWN_THRESHOLD) {
 		if (_param_rm_max_jerk.get() > FLT_EPSILON && _param_rm_max_accel.get() > FLT_EPSILON) {
 			desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rm_max_jerk.get(),
 					_param_rm_max_accel.get(), distance_to_next_wp, 0.0f);
@@ -143,81 +148,50 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con
 		mission_finished = true;
 	}
 
+	float forward_throttle{0.f};
+	float lateral_throttle{0.f};
+	float desired_yaw_rate{0.f};
+
 	if (!mission_finished) {
 		desired_velocity = desired_speed * Vector2f(cosf(heading_error), sinf(heading_error));
-	}
-
-
-	// // State machine
-	// if (!mission_finished && distance_to_next_wp > _param_nav_acc_rad.get()) {
-	// 	if (_currentState == GuidanceState::STOPPED) {
-	// 		_currentState = GuidanceState::DRIVING;
-	// 	}
 
-	// 	if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rm_trans_drv_trn.get()) {
-	// 		pid_reset_integral(&_pid_heading);
-	// 		_currentState = GuidanceState::SPOT_TURNING;
+		// Closed loop heading control
+		desired_yaw_rate = pid_calculate(&_pid_heading, desired_yaw, yaw, 0, dt);
 
-	// 	} else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rm_trans_trn_drv.get()) {
-	// 		pid_reset_integral(&_pid_heading);
-	// 		_currentState = GuidanceState::DRIVING;
-	// 	}
+		// Closed loop speed control
+		if (fabsf(desired_velocity(0)) < FLT_EPSILON) {
+			pid_reset_integral(&_pid_throttle);
 
-	// } else { // Mission finished or delay command
-	// 	_currentState = GuidanceState::STOPPED;
-	// }
+		} else {
+			forward_throttle = pid_calculate(&_pid_throttle, desired_velocity(0), forward_speed, 0,
+							 dt);
 
-	// // Guidance logic
-	// switch (_currentState) {
-	// case GuidanceState::DRIVING:
-	// case GuidanceState::SPOT_TURNING: {
-	// 		desired_speed = _param_rm_miss_spd_def.get();
-
-	// 		if (_param_rm_max_jerk.get() > FLT_EPSILON && _param_rm_max_accel.get() > FLT_EPSILON) {
-	// 			desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rm_max_jerk.get(),
-	// 					_param_rm_max_accel.get(), distance_to_next_wp, 0.0f);
-	// 			desired_speed = math::constrain(desired_speed, -_param_rm_max_speed.get(), _param_rm_max_speed.get());
-	// 		}
-
-	// 		desired_velocity = desired_speed * Vector2f(cosf(desired_heading), sinf(desired_heading));
-	// 		// lateral_throttle = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt);
-	// 	} break;
-
-	// // case GuidanceState::SPOT_TURNING:
-	// // 	if (actual_speed < TURN_MAX_VELOCITY) { // Wait for the rover to stop
-	// // 		desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); // Turn on the spot
-	// // 	}
-
-	// // 	break;
-
-	// case GuidanceState::STOPPED:
-	// default:
-	// 	desired_speed = 0.f;
-	// 	// desired_yaw_rate = 0.f;
-	// 	break;
-
-	// }
-
-	// Closed loop speed control
-	// float throttle{0.f};
+			if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
+				forward_throttle += math::interpolate<float>(desired_velocity(0),
+						    0.f, _param_rm_max_speed.get(),
+						    0.f, 1.f);
+			}
+		}
 
-	// if (fabsf(desired_speed) < FLT_EPSILON) {
-	// 	pid_reset_integral(&_pid_throttle);
+		if (fabsf(desired_velocity(1)) < FLT_EPSILON) {
+			pid_reset_integral(&_pid_lateral_throttle);
 
-	// } else {
-	// 	throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0,
-	// 				 dt);
+		} else {
+			lateral_throttle = pid_calculate(&_pid_lateral_throttle, desired_velocity(1), lateral_speed, 0,
+							 dt);
 
-	// 	if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
-	// 		throttle += math::interpolate<float>(desired_speed,
-	// 						     0.f, _param_rm_max_speed.get(),
-	// 						     0.f, 1.f);
-	// 	}
-	// }
+			if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
+				lateral_throttle += math::interpolate<float>(desired_velocity(1),
+						    0.f, _param_rm_max_speed.get(),
+						    0.f, 1.f);
+			}
+		}
+	}
 
 	// Publish mecanum controller status (logging)
 	_rover_mecanum_guidance_status.timestamp = _timestamp;
-	_rover_mecanum_guidance_status.desired_speed = desired_speed;
+	_rover_mecanum_guidance_status.desired_forward_speed = desired_velocity(0);
+	_rover_mecanum_guidance_status.desired_lateral_speed = desired_velocity(1);
 	_rover_mecanum_guidance_status.pid_throttle_integral = _pid_throttle.integral;
 	_rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance();
 	_rover_mecanum_guidance_status.pid_heading_integral = _pid_heading.integral;
@@ -228,14 +202,10 @@ RoverMecanumGuidance::mecanum_setpoint RoverMecanumGuidance::computeGuidance(con
 
 	// Return setpoints
 	mecanum_setpoint mecanum_setpoint_temp;
-	mecanum_setpoint_temp.forward_throttle = math::interpolate(desired_velocity(0), -_param_rm_max_speed.get(),
-			_param_rm_max_speed.get(), -0.5f, 0.5f);
-	mecanum_setpoint_temp.lateral_throttle = math::interpolate(desired_velocity(1), -_param_rm_max_speed.get(),
-			_param_rm_max_speed.get(), -0.5f, 0.5f);
-	mecanum_setpoint_temp.yaw_rate = 0.f;
+	mecanum_setpoint_temp.forward_throttle = math::constrain(forward_throttle, -1.f, 1.f);
+	mecanum_setpoint_temp.lateral_throttle = math::constrain(lateral_throttle, -1.f, 1.f);
+	mecanum_setpoint_temp.yaw_rate = desired_yaw_rate;
 	mecanum_setpoint_temp.closed_loop_yaw_rate = true;
-	printf("Forward: %f, Lateral: %f \n", (double)mecanum_setpoint_temp.forward_throttle,
-	       (double)mecanum_setpoint_temp.lateral_throttle);
 	return mecanum_setpoint_temp;
 
 }

src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp

@@ -90,11 +90,11 @@ class RoverMecanumGuidance : public ModuleParams
 	/**
 	 * @brief Compute guidance for the vehicle.
 	 * @param yaw The yaw orientation of the vehicle in radians.
-	 * @param actual_speed The velocity of the vehicle in m/s.
+	 * @param forward_speed The velocity of the vehicle in m/s.
 	 * @param dt The time step in seconds.
 	 * @param nav_state Navigation state of the rover.
 	 */
-	RoverMecanumGuidance::mecanum_setpoint computeGuidance(float yaw, float actual_speed,
+	RoverMecanumGuidance::mecanum_setpoint computeGuidance(float yaw, float forward_speed, float lateral_speed,
 			int nav_state);
 
 	/**
@@ -128,7 +128,6 @@ class RoverMecanumGuidance : public ModuleParams
 	float _max_yaw_rate{0.f};
 	float _theta{0.f};
 
-
 	// Waypoints
 	Vector2d _curr_pos{};
 	Vector2f _curr_pos_ned{};
@@ -143,9 +142,11 @@ class RoverMecanumGuidance : public ModuleParams
 	// Controllers
 	PID_t _pid_heading; // The PID controller for the heading
 	PID_t _pid_throttle; // The PID controller for velocity
+	PID_t _pid_lateral_throttle; // The PID controller for velocity
 
 	// Constants
-	static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s]
+	static constexpr float SLOW_DOWN_THRESHOLD =
+		2.61799f; // Slow down threshold for the maximum angle between the prev-curr and curr-next waypoint segments.
 
 	// Parameters
 	DEFINE_PARAMETERS(