mecanum: add control folder

msg/CMakeLists.txt

@@ -184,6 +184,7 @@ set(msg_files
 	RoverDifferentialGuidanceStatus.msg
 	RoverDifferentialStatus.msg
 	RoverMecanumGuidanceStatus.msg
+	RoverMecanumSetpoint.msg
 	RoverMecanumStatus.msg
 	Rpm.msg
 	RtlStatus.msg

msg/RoverMecanumGuidanceStatus.msg

@@ -1,11 +1,6 @@
 uint64 timestamp # time since system start (microseconds)
 
-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
-float32 pid_throttle_integral # Integral of the PID for the throttle during missions
-uint8   state_machine         # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED]
 
 # TOPICS rover_mecanum_guidance_status

msg/RoverMecanumSetpoint.msg

@@ -0,0 +1,11 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 forward_speed_setpoint # [m/s or normalized [-1, 1]] Desired forward speed for the rover
+float32 lateral_speed_setpoint # [m/s or normalized [-1, 1]] Desired lateral speed for the rover
+float32 yaw_rate_setpoint      # [rad/s or normalized [-1, 1]] Desired yaw rate for the rover
+float32 yaw_setpoint 	       # [rad] Desired yaw (heading) for the rover
+bool closed_loop_speed      # Use closed loop velocity controller
+bool closed_loop_yaw_rate      # Use closed loop yaw rate controller
+bool closed_loop_yaw           # Use closed loop yaw controller
+
+# TOPICS rover_mecanum_setpoint

msg/RoverMecanumStatus.msg

@@ -1,9 +1,12 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 forward_speed	       # [m/s] Actual forward speed of the rover
-float32 lateral_speed	       # [m/s] Actual lateral speed of the rover
+float32 actual_forward_speed   # [m/s] Actual forward speed of the rover
+float32 desired_forward_speed  # [m/s] Desired forward speed of the rover
+float32 actual_lateral_speed   # [m/s] Actual lateral speed of the rover
+float32 desired_lateral_speed  # [m/s] Desired 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 actual_yaw_deg         # [deg] Actual yaw of the rover
 float32 pid_yaw_rate_integral  # Integral of the PID for the desired yaw rate controller
 
 # TOPICS rover_mecanum_status

src/modules/logger/logged_topics.cpp

@@ -107,6 +107,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("rover_differential_guidance_status", 100);
 	add_optional_topic("rover_differential_status", 100);
 	add_optional_topic("rover_mecanum_guidance_status", 100);
+	add_optional_topic("rover_mecanum_setpoint", 100);
 	add_optional_topic("rover_mecanum_status", 100);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);

src/modules/rover_mecanum/CMakeLists.txt

@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(RoverMecanumGuidance)
+add_subdirectory(RoverMecanumControl)
 
 px4_add_module(
 	MODULE modules__rover_mecanum
@@ -41,6 +42,7 @@ px4_add_module(
 		RoverMecanum.hpp
 	DEPENDS
 		RoverMecanumGuidance
+		RoverMecanumControl
 		px4_work_queue
 		modules__control_allocator # for parameter CA_R_REV
 		pure_pursuit

src/modules/rover_mecanum/RoverMecanum.cpp

@@ -40,11 +40,7 @@ RoverMecanum::RoverMecanum() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	updateParams();
-	_rover_mecanum_status_pub.advertise();
-	pid_init(&_pid_yaw_rate, 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);
-	pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f);
+	_rover_mecanum_setpoint_pub.advertise();
 }
 
 bool RoverMecanum::init()
@@ -58,31 +54,6 @@ void RoverMecanum::updateParams()
 	ModuleParams::updateParams();
 
 	_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
-
-	pid_set_parameters(&_pid_yaw_rate,
-			   _param_rm_yaw_rate_p.get(), // Proportional gain
-			   _param_rm_yaw_rate_i.get(), // Integral gain
-			   0.f, // Derivative gain
-			   1.f, // Integral limit
-			   1.f); // Output limit
-	pid_set_parameters(&_pid_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
-	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
-	pid_set_parameters(&_pid_heading,
-			   _param_rm_p_gain_heading.get(),  // Proportional gain
-			   _param_rm_i_gain_heading.get(),  // Integral gain
-			   0.f,  // Derivative gain
-			   _max_yaw_rate,  // Integral limit
-			   _max_yaw_rate);  // Output limit
 }
 
 void RoverMecanum::Run()
@@ -96,156 +67,96 @@ void RoverMecanum::Run()
 	updateSubscriptions();
 
 	// Timestamps
-	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 = hrt_absolute_time();
 
 	// Navigation modes
 	switch (_nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
 			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
 
 			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				_mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle;
-				_mecanum_setpoint.lateral_throttle = manual_control_setpoint.roll;
-				_mecanum_setpoint.yaw_rate = manual_control_setpoint.yaw * _param_rm_man_yaw_scale.get();
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = manual_control_setpoint.throttle;
+				rover_mecanum_setpoint.lateral_speed_setpoint = manual_control_setpoint.roll;
+				rover_mecanum_setpoint.yaw_rate_setpoint = manual_control_setpoint.yaw * _param_rm_man_yaw_scale.get();
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+				rover_mecanum_setpoint.closed_loop_yaw_rate = false;
+				rover_mecanum_setpoint.closed_loop_speed = false;
+				rover_mecanum_setpoint.closed_loop_yaw = false;
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
 
 			}
-
-			_mecanum_setpoint.closed_loop_yaw_rate = false;
-			_mecanum_setpoint.closed_loop_speed = false;
 		} break;
 
 	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
 			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
 
 			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				_mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle;
-				_mecanum_setpoint.lateral_throttle = manual_control_setpoint.roll;
-				_mecanum_setpoint.yaw_rate = math::interpolate<float>(manual_control_setpoint.yaw,
-							     -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = manual_control_setpoint.throttle;
+				rover_mecanum_setpoint.lateral_speed_setpoint = manual_control_setpoint.roll;
+				rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.yaw,
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+				rover_mecanum_setpoint.closed_loop_yaw_rate = true;
+				rover_mecanum_setpoint.closed_loop_speed = false;
+				rover_mecanum_setpoint.closed_loop_yaw = false;
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
 			}
-
-			_mecanum_setpoint.closed_loop_yaw_rate = true;
-			_mecanum_setpoint.closed_loop_speed = false;
 		} break;
 
 	case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
 			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
 
 			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				_mecanum_setpoint.forward_throttle = manual_control_setpoint.throttle;
-				_mecanum_setpoint.lateral_throttle = manual_control_setpoint.roll;
-				_mecanum_setpoint.yaw_rate = math::interpolate<float>(manual_control_setpoint.yaw,
-							     -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-
-				if (_mecanum_setpoint.yaw_rate < FLT_EPSILON && _mecanum_setpoint.closed_loop_heading == false) {
-					_mecanum_setpoint.heading = _vehicle_yaw;
-					_mecanum_setpoint.closed_loop_heading = true;
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+						-2.f, 2.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get());
+				rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
+						-2.f, 2.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get());
+				rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.yaw,
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+
+				if (rover_mecanum_setpoint.yaw_rate_setpoint < FLT_EPSILON && _closed_loop_yaw == false) {
+					rover_mecanum_setpoint.yaw_setpoint = _vehicle_yaw;
+					rover_mecanum_setpoint.closed_loop_yaw = true;
+					_closed_loop_yaw = true;
 
 				} else {
-					_mecanum_setpoint.closed_loop_heading = false;
+					rover_mecanum_setpoint.closed_loop_yaw = false;
+					_closed_loop_yaw = false;
 				}
-			}
 
-			_mecanum_setpoint.closed_loop_yaw_rate = true;
-			_mecanum_setpoint.closed_loop_speed = true;
+				rover_mecanum_setpoint.closed_loop_yaw_rate = true;
+				rover_mecanum_setpoint.closed_loop_speed = true;
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+			}
 		} break;
 
 	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);
+		_rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _vehicle_lateral_speed, _nav_state);
 		break;
 
 	default: // Unimplemented nav states will stop the rover
-		_mecanum_setpoint.forward_throttle = 0.f;
-		_mecanum_setpoint.lateral_throttle = 0.f;
-		_mecanum_setpoint.yaw_rate = 0.f;
-		_mecanum_setpoint.closed_loop_yaw_rate = false;
-		_mecanum_setpoint.closed_loop_speed = false;
+		rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+		rover_mecanum_setpoint.timestamp = timestamp;
+		rover_mecanum_setpoint.forward_speed_setpoint = 0.f;
+		rover_mecanum_setpoint.lateral_speed_setpoint = 0.f;
+		rover_mecanum_setpoint.yaw_rate_setpoint = 0.f;
+		rover_mecanum_setpoint.yaw_setpoint = NAN;
+		rover_mecanum_setpoint.closed_loop_yaw_rate = false;
+		rover_mecanum_setpoint.closed_loop_speed = false;
+		rover_mecanum_setpoint.closed_loop_yaw = false;
+		_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
 		break;
 	}
 
-	float speed_diff_normalized = _mecanum_setpoint.yaw_rate;
-
-	// Closed loop heading control
-	if (_mecanum_setpoint.closed_loop_heading) {
-		_mecanum_setpoint.yaw_rate = pid_calculate(&_pid_heading, _mecanum_setpoint.heading, _vehicle_yaw, 0, dt);
-	}
-
-	// Closed loop yaw rate control
-	if (_mecanum_setpoint.closed_loop_yaw_rate) {
-		if (fabsf(_mecanum_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD
-		    || fabsf(_mecanum_setpoint.yaw_rate) < FLT_EPSILON) {
-			speed_diff_normalized = 0.f;
-			pid_reset_integral(&_pid_yaw_rate);
-
-		} else {
-			const float speed_diff = _mecanum_setpoint.yaw_rate * _param_rm_wheel_track.get(); // Feedforward
-			speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rm_max_speed.get(),
-						_param_rm_max_speed.get(), -1.f, 1.f);
-			speed_diff_normalized = math::constrain(speed_diff_normalized +
-								pid_calculate(&_pid_yaw_rate, _mecanum_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt),
-								-1.f, 1.f); // Feedback
-		}
-
-	} else {
-		pid_reset_integral(&_pid_yaw_rate);
-	}
-
-	// Closed loop speed control
-	float forward_throttle = _mecanum_setpoint.forward_throttle;
-	float lateral_throttle = _mecanum_setpoint.lateral_throttle;
-
-	if (_mecanum_setpoint.closed_loop_speed) {
-		if (fabsf(_mecanum_setpoint.forward_throttle) < FLT_EPSILON) {
-			pid_reset_integral(&_pid_throttle);
-
-		} else {
-			forward_throttle = pid_calculate(&_pid_throttle, _mecanum_setpoint.forward_throttle, _vehicle_forward_speed, 0,
-							 dt);
-
-			if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
-				forward_throttle += math::interpolate<float>(_mecanum_setpoint.forward_throttle,
-						    0.f, _param_rm_max_speed.get(),
-						    0.f, 1.f);
-			}
-		}
-
-		if (fabsf(_mecanum_setpoint.lateral_throttle) < FLT_EPSILON) {
-			pid_reset_integral(&_pid_lateral_throttle);
-
-		} else {
-			lateral_throttle = pid_calculate(&_pid_lateral_throttle, _mecanum_setpoint.lateral_throttle, _vehicle_lateral_speed, 0,
-							 dt);
-
-			if (_param_rm_max_speed.get() > FLT_EPSILON) { // Feed-forward term
-				lateral_throttle += math::interpolate<float>(_mecanum_setpoint.lateral_throttle,
-						    0.f, _param_rm_max_speed.get(),
-						    0.f, 1.f);
-			}
-		}
-	}
-
-	// Publish rover mecanum status (logging)
-	rover_mecanum_status_s rover_mecanum_status{};
-	rover_mecanum_status.timestamp = _timestamp;
-	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;
-	_rover_mecanum_status_pub.publish(rover_mecanum_status);
-
-	// Publish to motors
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get();
-	computeMotorCommands(forward_throttle, lateral_throttle,
-			     speed_diff_normalized).copyTo(actuator_motors.control);
-	actuator_motors.timestamp = _timestamp;
-	_actuator_motors_pub.publish(actuator_motors);
+	_rover_mecanum_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed,
+			_vehicle_lateral_speed);
 
 }
 
@@ -266,7 +177,7 @@ void RoverMecanum::updateSubscriptions()
 	if (_vehicle_angular_velocity_sub.updated()) {
 		vehicle_angular_velocity_s vehicle_angular_velocity{};
 		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
-		_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
+		_vehicle_yaw_rate = vehicle_angular_velocity.xyz[2];
 	}
 
 	if (_vehicle_attitude_sub.updated()) {
@@ -286,52 +197,6 @@ void RoverMecanum::updateSubscriptions()
 	}
 }
 
-matrix::Vector4f RoverMecanum::computeMotorCommands(float forward_speed, float lateral_speed, float speed_diff)
-{
-	// Prioritize ratio between forward and lateral speed over either magnitude
-	float combined_speed =  fabsf(forward_speed) + fabsf(lateral_speed);
-
-	if (combined_speed > 1.f) {
-		forward_speed /= combined_speed;
-		lateral_speed /= combined_speed;
-		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);
-		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);
-	const float m_data[12] = {1, -1, -1, 1, 1, 1, 1, 1, -1, 1, -1, 1};
-	const Matrix<float, 4, 3> m(m_data);
-	const Vector4f motor_commands = m * input;
-
-	return motor_commands;
-}
-
 int RoverMecanum::task_spawn(int argc, char *argv[])
 {
 	RoverMecanum *instance = new RoverMecanum();