differential: add individual parameters for speed and yaw rate feedfo…

…rward

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -16,11 +16,12 @@ param set-default RD_WHEEL_TRACK 0.3
 param set-default RD_MAN_YAW_SCALE 0.1
 param set-default RD_MAX_ACCEL 6
 param set-default RD_MAX_JERK 30
-param set-default RD_MAX_SPEED 7
-param set-default RD_YAW_RATE_P 5
+param set-default RD_MAX_THR_YAW_R 5
+param set-default RD_YAW_RATE_P 0.1
 param set-default RD_YAW_RATE_I 0
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0
+param set-default RD_MAX_THR_SPD 7
 param set-default RD_SPEED_P 1
 param set-default RD_SPEED_I 0
 param set-default RD_MAX_YAW_RATE 180

ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover

@@ -25,12 +25,13 @@ param set-default RBCLW_REV 1 # reverse right wheels
 param set-default RD_WHEEL_TRACK 0.3
 param set-default RD_MAN_YAW_SCALE 1
 param set-default RD_MAX_ACCEL 5
-param set-default RD_MAX_JERK 50
-param set-default RD_MAX_SPEED 2
+param set-default RD_MAX_JERK 10
+param set-default RD_MAX_THR_YAW_R 4
 param set-default RD_YAW_RATE_P 0.1
 param set-default RD_YAW_RATE_I 0
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0
+param set-default RD_MAX_THR_SPD 2
 param set-default RD_SPEED_P 0.5
 param set-default RD_SPEED_I 0.1
 param set-default RD_MAX_YAW_RATE 300

msg/RoverDifferentialStatus.msg

@@ -1,11 +1,13 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 actual_speed	       # [m/s] Actual forward speed of the rover
-float32 actual_yaw_deg         # [deg] Actual yaw of the rover
-float32 actual_yaw_rate_deg_s  # [deg/s] Actual yaw rate of the rover
-float32 desired_yaw_rate_deg_s # [deg/s] Yaw rate setpoint for the closed loop yaw rate controller
-float32 pid_yaw_integral       # Integral of the PID for the closed loop yaw controller
-float32 pid_yaw_rate_integral  # Integral of the PID for the closed loop yaw rate controller
-float32 pid_throttle_integral  # Integral of the PID for the closed loop speed controller
+float32 actual_speed	         # [m/s] Actual forward speed of the rover
+float32 actual_yaw               # [rad] Actual yaw of the rover
+float32 actual_yaw_rate          # [rad/s] Actual yaw rate of the rover
+float32 desired_yaw_rate         # [rad/s] Yaw rate setpoint for the closed loop yaw rate controller
+float32 forward_speed_normalized # [-1, 1] Normalized forward speed setpoint
+float32 speed_diff_normalized    # [-1, 1] Normalized speed difference setpoint between the left and right motor
+float32 pid_yaw_integral         # Integral of the PID for the closed loop yaw controller
+float32 pid_yaw_rate_integral    # Integral of the PID for the closed loop yaw rate controller
+float32 pid_throttle_integral    # Integral of the PID for the closed loop speed controller
 
 # TOPICS rover_differential_status

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -92,9 +92,13 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	float speed_diff_normalized{0.f};
 
 	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
-		const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get(); // Feedforward
-		speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_speed.get(),
-					_param_rd_max_speed.get(), -1.f, 1.f);
+		if (_param_rd_wheel_track.get() > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward
+			const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get() /
+						 2.f;
+			speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_thr_yaw_r.get(),
+						_param_rd_max_thr_yaw_r.get(), -1.f, 1.f);
+		}
+
 		speed_diff_normalized = math::constrain(speed_diff_normalized +
 							pid_calculate(&_pid_yaw_rate, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt),
 							-1.f, 1.f); // Feedback
@@ -105,30 +109,34 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	}
 
 	// Speed control
-	float throttle{0.f};
+	float forward_speed_normalized{0.f};
 
 	if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control
-		if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feedforward
-			throttle += math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
-							     0.f, _param_rd_max_speed.get(),
-							     0.f, 1.f);
+		if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+			forward_speed_normalized = math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
+						   -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(),
+						   -1.f, 1.f);
 		}
 
-		throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0,
-					 dt); // Feedback
+		forward_speed_normalized = math::constrain(forward_speed_normalized + pid_calculate(&_pid_throttle,
+					   _rover_differential_setpoint.forward_speed_setpoint,
+					   vehicle_forward_speed, 0,
+					   dt), -1.f, 1.f); // Feedback
 
 	} else { // Use normalized setpoint
-		throttle = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ?
-			   math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
+		forward_speed_normalized = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ?
+					   math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.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.actual_yaw_deg = M_RAD_TO_DEG_F * vehicle_yaw;
-	rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _rover_differential_setpoint.yaw_rate_setpoint;
-	rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * vehicle_yaw_rate;
+	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;
@@ -137,7 +145,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	// Publish to motors
 	actuator_motors_s actuator_motors{};
 	actuator_motors.reversible_flags = _param_r_rev.get();
-	computeInverseKinematics(throttle, speed_diff_normalized).copyTo(actuator_motors.control);
+	computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control);
 	actuator_motors.timestamp = _timestamp;
 	_actuator_motors_pub.publish(actuator_motors);
 

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp

@@ -113,7 +113,8 @@ class RoverDifferentialControl : public ModuleParams
 	// Parameters
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
-		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
+		(ParamFloat<px4::params::RD_MAX_THR_SPD>) _param_rd_max_thr_spd,
+		(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
 		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
 		(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_yaw_rate_p,
 		(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_yaw_rate_i,

src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp

@@ -216,7 +216,7 @@ void RoverDifferentialGuidance::updateWaypoints()
 
 	// Waypoint cruising speed
 	if (position_setpoint_triplet.current.cruising_speed > 0.f) {
-		_max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get());
+		_max_forward_speed = position_setpoint_triplet.current.cruising_speed;
 
 	} else {
 		_max_forward_speed = _param_rd_miss_spd_def.get();

src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp

@@ -140,7 +140,6 @@ class RoverDifferentialGuidance : public ModuleParams
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
 		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
 		(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
 		(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,

src/modules/rover_differential/module.yaml

@@ -113,10 +113,14 @@ parameters:
         decimal: 2
         default: 0.5
 
-      RD_MAX_SPEED:
+      RD_MAX_THR_SPD:
         description:
-          short: Maximum speed the rover can drive
-          long: This parameter is used to map desired speeds to normalized motor commands.
+          short: Speed the rover drives at maximum throttle
+          long: |
+            This parameter is used to calculate the feedforward term of the closed loop speed control which linearly
+            maps desired speeds to normalized motor commands [-1. 1].
+            A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode.
+            Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower.
         type: float
         unit: m/s
         min: 0
@@ -138,6 +142,23 @@ parameters:
         decimal: 2
         default: 90
 
+      RD_MAX_THR_YAW_R:
+        description:
+          short: Yaw rate turning left/right wheels at max speed in opposite directions
+          long: |
+            This parameter is used to calculate the feedforward term of the closed loop yaw rate control.
+            The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate.
+            This desired speed difference is then linearly mapped to normalized motor commands.
+            A good starting point is twice the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)).
+            Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 2
+
       RD_MISS_SPD_DEF:
         description:
           short: Default forward speed for the rover during auto modes