differential: save

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -123,17 +123,88 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 					   vehicle_forward_speed, dt);
 
 	} else if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint
-		if (_param_rd_max_accel.get() > FLT_EPSILON && _param_rd_max_thr_spd.get() > FLT_EPSILON) { // Apply slew rates
-			_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_rd_max_accel.get() / _param_rd_max_thr_spd.get());
+		if (fabsf(_rover_differential_setpoint.forward_speed_setpoint_normalized) >= fabsf(
+			    _forward_speed_setpoint_with_accel_limit.getState())) {
+			if (_param_rd_max_accel.get() > FLT_EPSILON && _param_rd_max_thr_spd.get() > FLT_EPSILON) {
+				_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_rd_max_accel.get() / _param_rd_max_thr_spd.get());
+				_forward_speed_setpoint_with_accel_limit.update(_rover_differential_setpoint.forward_speed_setpoint_normalized, dt);
+
+			} else {
+				_forward_speed_setpoint_with_accel_limit.setForcedValue(_rover_differential_setpoint.forward_speed_setpoint_normalized);
+
+			}
+
+		} else if (_param_rd_max_decel.get() > FLT_EPSILON && _param_rd_max_thr_spd.get() > FLT_EPSILON) {
+			_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_rd_max_decel.get() / _param_rd_max_thr_spd.get());
 			_forward_speed_setpoint_with_accel_limit.update(_rover_differential_setpoint.forward_speed_setpoint_normalized, dt);
-			forward_speed_normalized = math::constrain(_forward_speed_setpoint_with_accel_limit.getState(), -1.f, 1.f);
 
 		} else {
-			forward_speed_normalized = math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f);
+			_forward_speed_setpoint_with_accel_limit.setForcedValue(_rover_differential_setpoint.forward_speed_setpoint_normalized);
+		}
+
+		forward_speed_normalized = math::constrain(_forward_speed_setpoint_with_accel_limit.getState(), -1.f, 1.f);
+
+	}
+
+	// Publish rover differential status (logging)
+	rover_differential_status_s rover_differential_status{};
+	rover_differential_status.timestamp = _timestamp;
+	rover_differential_status.actual_speed = vehicle_forward_speed;
+	rover_differential_status.desired_speed = _forward_speed_setpoint_with_accel_limit.getState();
+	rover_differential_status.actual_yaw = vehicle_yaw;
+	rover_differential_status.desired_yaw_rate = _rover_differential_setpoint.yaw_rate_setpoint;
+	rover_differential_status.actual_yaw_rate = vehicle_yaw_rate;
+	rover_differential_status.forward_speed_normalized = forward_speed_normalized;
+	rover_differential_status.speed_diff_normalized = speed_diff_normalized;
+	rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
+	rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
+	rover_differential_status.pid_yaw_integral = _pid_yaw.integral;
+	_rover_differential_status_pub.publish(rover_differential_status);
+
+	// Publish to motors
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get();
+	computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control);
+	actuator_motors.timestamp = _timestamp;
+	_actuator_motors_pub.publish(actuator_motors);
+}
+
+float RoverDifferentialControl::closedLoopSpeedControl(const float forward_speed_setpoint,
+		const float vehicle_forward_speed, const float dt)
+{
+	// Apply acceleration and deceleration limit
+	if (fabsf(forward_speed_setpoint) >= fabsf(_forward_speed_setpoint_with_accel_limit.getState())) {
+		if (_param_rd_max_accel.get() > FLT_EPSILON) {
+			_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_rd_max_accel.get());
+			_forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
+
+		} else {
+			_forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
+
 		}
+
+	} else if (_param_rd_max_decel.get() > FLT_EPSILON) {
+		_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_rd_max_decel.get());
+		_forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
+
+	} else {
+		_forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
 	}
 
+	// Closed loop speed control
+	float forward_speed_normalized{0.f};
+
+	if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+		forward_speed_normalized = math::interpolate<float>(_forward_speed_setpoint_with_accel_limit.getState(),
+					   -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(),
+					   -1.f, 1.f);
+	}
+
+	forward_speed_normalized += pid_calculate(&_pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
+				    vehicle_forward_speed, 0, dt); // Feedback
+
 	return math::constrain(forward_speed_normalized, -1.f, 1.f);
+
 }
 
 matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized,

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp

@@ -93,6 +93,7 @@ class RoverDifferentialControl : public ModuleParams
 	 * @return Normalized forward speed setpoint with applied slew rates [-1, 1].
 	 */
 	float closedLoopSpeedControl(float forward_speed_setpoint, float vehicle_forward_speed, float dt);
+
 	/**
 	 * @brief Compute normalized motor commands based on normalized setpoints.
 	 *