differential: add slew rates for setpoints, deprecate RD_MAN_YAW_SCAL…

…E and update airframe tuning
PX4 · Nov 11, 2024 · 4f00df6 · 4f00df6
1 parent fbbfcdb
commit 4f00df6
Show file tree

Hide file tree

Showing 12 changed files with 300 additions and 125 deletions.
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
@@ -13,22 +13,23 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
 
 # Rover parameters
 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_ACCEL 5
+param set-default RD_MAX_DECEL 10
 param set-default RD_MAX_JERK 30
-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_MAX_THR_YAW_R 1.5
+param set-default RD_YAW_RATE_P 0.25
+param set-default RD_YAW_RATE_I 0.01
 param set-default RD_YAW_P 5
-param set-default RD_YAW_I 0
-param set-default RD_MAX_SPEED 5
-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_YAW_I 0.1
+param set-default RD_MAX_SPEED 2
+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 5
+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
 
 # Pure pursuit parameters
 param set-default PP_LOOKAHD_MAX 30
@@ -43,13 +44,13 @@ param set-default SENS_EN_ARSPDSIM 0
 
 # Actuator mapping
 param set-default SIM_GZ_WH_FUNC1 101 # right wheel
-param set-default SIM_GZ_WH_MIN1 0
-param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_MIN1 70
+param set-default SIM_GZ_WH_MAX1 130
 param set-default SIM_GZ_WH_DIS1 100
 
 param set-default SIM_GZ_WH_FUNC2 102 # left wheel
-param set-default SIM_GZ_WH_MIN2 0
-param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_MIN2 70
+param set-default SIM_GZ_WH_MAX2 130
 param set-default SIM_GZ_WH_DIS2 100
 
 param set-default SIM_GZ_WH_REV 1 # reverse right wheel
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
@@ -21,7 +21,6 @@ param set-default COM_ARM_WO_GPS 1
 
 # Rover parameters
 param set-default RD_WHEEL_TRACK 0.9
-param set-default RD_MAN_YAW_SCALE 0.1
 param set-default RD_YAW_RATE_I 0.1
 param set-default RD_YAW_RATE_P 5
 param set-default RD_MAX_ACCEL 1

diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover
@@ -23,22 +23,24 @@ param set-default RBCLW_REV 1 # reverse right wheels
 
 # Rover parameters
 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 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_MAX_ACCEL 3
+param set-default RD_MAX_DECEL 4
+param set-default RD_MAX_JERK 5
+param set-default RD_MAX_SPEED 1.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
+param set-default RD_TRANS_TRN_DRV 0.139626
 param set-default RD_YAW_P 5
-param set-default RD_YAW_I 0
-param set-default RD_MAX_SPEED 1.8
-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
-param set-default RD_MISS_SPD_DEF 1.8
-param set-default RD_TRANS_DRV_TRN 0.349066
-param set-default RD_TRANS_TRN_DRV 0.174533
+param set-default RD_YAW_I 0.1
+param set-default RD_YAW_RATE_P 0.1
+param set-default RD_YAW_RATE_I 0.01
+
 
 # Pure pursuit parameters
 param set-default PP_LOOKAHD_MAX 10

diff --git a/msg/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg
@@ -3,7 +3,7 @@ uint64 timestamp # time since system start (microseconds)
 float32 forward_speed_setpoint 		  # [m/s] Desired forward speed for the rover
 float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover
 float32 yaw_rate_setpoint      		  # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used)
-float32 yaw_rate_setpoint_normalized      # [-1, 1] Normalized yaw rate for the rover
+float32 speed_diff_setpoint_normalized    # [-1, 1] Normalized speed difference between the left and right wheels
 float32 yaw_setpoint 	       		  # [rad] Desired yaw (heading) for the rover
 
 # TOPICS rover_differential_setpoint
diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg
@@ -1,13 +1,14 @@
 uint64 timestamp # time since system start (microseconds)
 
-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
+float32 measured_forward_speed	        # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ
+float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate
+float32 measured_yaw                    # [rad] Measured yaw
+float32 adjusted_yaw_setpoint           # [rad] Yaw setpoint after applying slew rate
+float32 clyaw_yaw_rate_setpoint         # [rad/s] Yaw rate setpoint output by the closed loop yaw controller
+float32 measured_yaw_rate               # [rad/s] Measured yaw rate
+float32 adjusted_yaw_rate_setpoint      # [rad/s] Yaw rate setpoint from the closed loop yaw 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
 
 # TOPICS rover_differential_status
diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp
@@ -63,7 +63,7 @@ void RoverDifferential::Run()
 
 	updateSubscriptions();
 
-	// Generate and publish attitude and velocity setpoints
+	// Generate and publish attitude, rate and speed setpoints
 	hrt_abstime timestamp = hrt_absolute_time();
 
 	switch (_nav_state) {
@@ -76,7 +76,19 @@ void RoverDifferential::Run()
 				rover_differential_setpoint.forward_speed_setpoint = NAN;
 				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
 				rover_differential_setpoint.yaw_setpoint = NAN;
-				rover_differential_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get();
+
+				if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) {
+					const float scaled_yaw_rate_input = math::interpolate<float>(manual_control_setpoint.roll,
+									    -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+					const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f;
+					rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate<float>(speed_diff,
+							-_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f);
+
+				} else {
+					rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll;
+
+				}
+
 				rover_differential_setpoint.yaw_rate_setpoint = NAN;
 				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 			}
@@ -92,7 +104,7 @@ void RoverDifferential::Run()
 				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
 				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
 						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-				rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
 				rover_differential_setpoint.yaw_setpoint = NAN;
 				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 			}
@@ -109,7 +121,7 @@ void RoverDifferential::Run()
 				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
 				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
 						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-				rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
 				rover_differential_setpoint.yaw_setpoint = NAN;
 
 				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
@@ -144,7 +156,7 @@ void RoverDifferential::Run()
 				rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
 				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
 						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-				rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
 				rover_differential_setpoint.yaw_setpoint = NAN;
 
 				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
@@ -191,7 +203,7 @@ void RoverDifferential::Run()
 		rover_differential_setpoint.forward_speed_setpoint = NAN;
 		rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
 		rover_differential_setpoint.yaw_rate_setpoint = NAN;
-		rover_differential_setpoint.yaw_rate_setpoint_normalized = 0.f;
+		rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f;
 		rover_differential_setpoint.yaw_setpoint = NAN;
 		_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 		break;

diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp
@@ -129,9 +129,10 @@ class RoverDifferential : public ModuleBase<RoverDifferential>, public ModulePar
 	Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
-		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
-		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
-		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
+		(ParamFloat<px4::params::RD_WHEEL_TRACK>)   _param_rd_wheel_track,
+		(ParamFloat<px4::params::RD_MAX_YAW_RATE>)  _param_rd_max_yaw_rate,
+		(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
+		(ParamFloat<px4::params::RD_MAX_SPEED>)     _param_rd_max_speed,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max
 	)
 };