Differential: Fix PID implementation and adjust speed setpoint based on yaw rate setpoint

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

@@ -26,7 +26,6 @@ param set-default RD_MAX_THR_SPD 2.15
 param set-default RD_SPEED_P 0.1
 param set-default RD_SPEED_I 0.01
 param set-default RD_MAX_YAW_RATE 180
-param set-default RD_MISS_SPD_DEF 2
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 param set-default RD_MAX_YAW_ACCEL 1000

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -29,7 +29,6 @@ param set-default RD_MAX_SPEED 8
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0.1
 param set-default RD_MAX_YAW_RATE 30
-param set-default RD_MISS_SPD_DEF 8
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 

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

@@ -31,7 +31,6 @@ param set-default RD_MAX_THR_SPD 1.9
 param set-default RD_MAX_THR_YAW_R 0.7
 param set-default RD_MAX_YAW_ACCEL 600
 param set-default RD_MAX_YAW_RATE 250
-param set-default RD_MISS_SPD_DEF 1.5
 param set-default RD_SPEED_P 0.1
 param set-default RD_SPEED_I 0.01
 param set-default RD_TRANS_DRV_TRN 0.785398

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -58,19 +58,19 @@ void RoverDifferentialControl::updateParams()
 			   _param_rd_yaw_rate_p.get(), // Proportional gain
 			   _param_rd_yaw_rate_i.get(), // Integral gain
 			   0.f, // Derivative gain
-			   1.f, // Integral limit
+			   _param_rd_yaw_rate_i.get() > FLT_EPSILON ? 1.f / _param_rd_yaw_rate_i.get() : 0.f, // Integral limit
 			   1.f); // Output limit
 	pid_set_parameters(&_pid_throttle,
-			   _param_rd_p_gain_speed.get(), // Proportional gain
-			   _param_rd_i_gain_speed.get(), // Integral gain
+			   _param_rd_speed_p.get(), // Proportional gain
+			   _param_rd_speed_i.get(), // Integral gain
 			   0.f, // Derivative gain
-			   1.f, // Integral limit
+			   _param_rd_speed_i.get() > FLT_EPSILON ? 1.f / _param_rd_speed_i.get() : 0.f, // Integral limit
 			   1.f); // Output limit
 	pid_set_parameters(&_pid_yaw,
-			   _param_rd_p_gain_yaw.get(),  // Proportional gain
-			   _param_rd_i_gain_yaw.get(),  // Integral gain
+			   _param_rd_yaw_p.get(),  // Proportional gain
+			   _param_rd_yaw_i.get(),  // Integral gain
 			   0.f,  // Derivative gain
-			   _max_yaw_rate,  // Integral limit
+			   _param_rd_yaw_i.get() > FLT_EPSILON ? _max_yaw_rate / _param_rd_yaw_i.get() : 0.f, // Integral limit
 			   _max_yaw_rate);  // Output limit
 
 	// Update slew rates
@@ -126,6 +126,20 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	float forward_speed_normalized{0.f};
 
 	if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) {
+		if (_param_rd_max_thr_spd.get() > FLT_EPSILON) {
+
+			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);
+
+			if (fabsf(forward_speed_normalized) > 1.f -
+			    fabsf(speed_diff_normalized)) { // Adjust forward speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels
+				_rover_differential_setpoint.forward_speed_setpoint = sign(_rover_differential_setpoint.forward_speed_setpoint) *
+						math::interpolate<float>(1.f - fabsf(speed_diff_normalized), -1.f, 1.f,
+								-_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get());
+			}
+		}
+
+
 		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint,
 					   vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle,
 					   _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false);
@@ -142,9 +156,9 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	_rover_differential_status.measured_forward_speed = vehicle_forward_speed;
 	_rover_differential_status.measured_yaw = vehicle_yaw;
 	_rover_differential_status.measured_yaw_rate = vehicle_yaw_rate;
-	_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.pid_yaw_rate_integral = _pid_yaw_rate.integral * _param_rd_yaw_rate_i.get();
+	_rover_differential_status.pid_throttle_integral = _pid_throttle.integral * _param_rd_speed_i.get();
+	_rover_differential_status.pid_yaw_integral = _pid_yaw.integral * _param_rd_yaw_i.get();
 	_rover_differential_status_pub.publish(_rover_differential_status);
 
 	// Publish to motors

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp

@@ -160,10 +160,10 @@ class RoverDifferentialControl : public ModuleParams
 		(ParamFloat<px4::params::RD_MAX_YAW_ACCEL>) _param_rd_max_yaw_accel,
 		(ParamFloat<px4::params::RD_YAW_RATE_P>)    _param_rd_yaw_rate_p,
 		(ParamFloat<px4::params::RD_YAW_RATE_I>)    _param_rd_yaw_rate_i,
-		(ParamFloat<px4::params::RD_SPEED_P>)       _param_rd_p_gain_speed,
-		(ParamFloat<px4::params::RD_SPEED_I>)       _param_rd_i_gain_speed,
-		(ParamFloat<px4::params::RD_YAW_P>)         _param_rd_p_gain_yaw,
-		(ParamFloat<px4::params::RD_YAW_I>)         _param_rd_i_gain_yaw,
+		(ParamFloat<px4::params::RD_SPEED_P>)       _param_rd_speed_p,
+		(ParamFloat<px4::params::RD_SPEED_I>)       _param_rd_speed_i,
+		(ParamFloat<px4::params::RD_YAW_P>)         _param_rd_yaw_p,
+		(ParamFloat<px4::params::RD_YAW_I>)         _param_rd_yaw_i,
 		(ParamInt<px4::params::CA_R_REV>)           _param_r_rev
 	)
 };

src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp

@@ -40,7 +40,7 @@ using namespace matrix;
 RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent)
 {
 	updateParams();
-	_max_forward_speed = _param_rd_miss_spd_def.get();
+	_max_forward_speed = _param_rd_max_speed.get();
 	_rover_differential_guidance_status_pub.advertise();
 }
 
@@ -96,6 +96,16 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f
 								_param_rd_max_decel.get(), distance_to_curr_wp, 0.0f);
 					desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed);
 				}
+
+			} else if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON
+				   && _param_rd_miss_spd_gain.get() > FLT_EPSILON) {
+				const float speed_reduction = math::constrain(_param_rd_miss_spd_gain.get() * math::interpolate(
+								      M_PI_F - _waypoint_transition_angle, 0.f,
+								      M_PI_F, 0.f, 1.f), 0.f, 1.f);
+				desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(),
+							_param_rd_max_decel.get(), distance_to_curr_wp, _max_forward_speed * (1.f - speed_reduction));
+				desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed,
+									_max_forward_speed);
 			}
 
 		} break;
@@ -219,6 +229,6 @@ void RoverDifferentialGuidance::updateWaypoints()
 		_max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get());
 
 	} else {
-		_max_forward_speed = _param_rd_miss_spd_def.get();
+		_max_forward_speed = _param_rd_max_speed.get();
 	}
 }

src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp

@@ -144,9 +144,8 @@ class RoverDifferentialGuidance : public ModuleParams
 		(ParamFloat<px4::params::RD_MAX_JERK>)      _param_rd_max_jerk,
 		(ParamFloat<px4::params::RD_MAX_DECEL>)     _param_rd_max_decel,
 		(ParamFloat<px4::params::RD_MAX_SPEED>)     _param_rd_max_speed,
-		(ParamFloat<px4::params::RD_MISS_SPD_DEF>)  _param_rd_miss_spd_def,
 		(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
-		(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn
-
+		(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
+		(ParamFloat<px4::params::RD_MISS_SPD_GAIN>) _param_rd_miss_spd_gain
 	)
 };

src/modules/rover_differential/module.yaml

@@ -191,17 +191,6 @@ parameters:
         decimal: 2
         default: 2
 
-      RD_MISS_SPD_DEF:
-        description:
-          short: Default forward speed for the rover during auto modes
-        type: float
-        unit: m/s
-        min: 0
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: 1
-
       RD_TRANS_TRN_DRV:
         description:
             short: Yaw error threshhold to switch from spot turning to driving
@@ -228,3 +217,19 @@ parameters:
         increment: 0.01
         decimal: 3
         default: 0.174533
+
+      RD_MISS_SPD_GAIN:
+        description:
+          short: Tuning parameter for the speed reduction during waypoint transition
+          long: |
+            The waypoint transition speed is calculated as:
+            Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN)
+            The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP
+            interpolated from [0, 180] -> [0, 1].
+            Higher value -> More speed reduction during waypoint transitions.
+        type: float
+        min: 0.05
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1