Differential Rover: Refactor and clean up, align with Ackermann rover(#…

…24318)

* differential: refactor code architecture

* Offboard fix

* fix accel/decel slew rate

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

@@ -11,29 +11,39 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
 
 param set-default SIM_GZ_EN 1 # Gazebo bridge
 
-# Rover parameters
+param set-default NAV_ACC_RAD 0.5
+
+# Differential Parameters
 param set-default RD_WHEEL_TRACK 0.3
-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 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.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_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
-param set-default PP_LOOKAHD_MIN 2
+# Rover Control Parameters
+param set-default RO_ACCEL_LIM 5
+param set-default RO_DECEL_LIM 10
+param set-default RO_JERK_LIM 30
+param set-default RO_MAX_THR_SPEED 2.1
+
+# Rover Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.01
+param set-default RO_YAW_RATE_P 0.25
+param set-default RO_YAW_RATE_LIM 180
+param set-default RO_YAW_ACCEL_LIM 120
+param set-default RO_YAW_DECEL_LIM 1000
+
+# Rover Attitude Control Parameters
+param set-default RO_YAW_P 5
+
+# Rover Position Control Parameters
+param set-default RO_SPEED_LIM 2
+param set-default RO_SPEED_I 0.5
+param set-default RO_SPEED_P 1
+
+# Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1
 
 # Simulated sensors
 param set-default SENS_EN_GPSSIM 1

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

@@ -21,26 +21,37 @@ param set-default RBCLW_FUNC1 101
 param set-default RBCLW_FUNC2 102
 param set-default RBCLW_REV 1 # reverse right wheels
 
-# Rover parameters
+
+param set-default NAV_ACC_RAD 0.5
+
+# Differential Parameters
 param set-default RD_WHEEL_TRACK 0.3
-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_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.1
-param set-default RD_YAW_RATE_P 0.1
-param set-default RD_YAW_RATE_I 0.01
 
-# Pure pursuit parameters
+# Rover Control Parameters
+param set-default RO_ACCEL_LIM 3
+param set-default RO_DECEL_LIM 4
+param set-default RO_JERK_LIM 5
+param set-default RO_MAX_THR_SPEED 1.9
+
+# Rover Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.01
+param set-default RO_YAW_RATE_P 0.1
+param set-default RO_YAW_RATE_LIM 250
+param set-default RO_YAW_ACCEL_LIM 600
+param set-default RO_YAW_DECEL_LIM 600
+
+# Rover Attitude Control Parameters
+param set-default RO_YAW_P 5
+
+# Rover Position Control Parameters
+param set-default RO_SPEED_LIM 1.6
+param set-default RO_SPEED_I 0.01
+param set-default RO_SPEED_P 0.1
+
+# Pure Pursuit parameters
+param set-default PP_LOOKAHD_GAIN 1
 param set-default PP_LOOKAHD_MAX 10
 param set-default PP_LOOKAHD_MIN 1
-param set-default PP_LOOKAHD_GAIN 1

msg/CMakeLists.txt

@@ -179,9 +179,6 @@ set(msg_files
 	RoverRateStatus.msg
 	RoverSteeringSetpoint.msg
 	RoverThrottleSetpoint.msg
-	RoverDifferentialGuidanceStatus.msg
-	RoverDifferentialSetpoint.msg
-	RoverDifferentialStatus.msg
 	RoverMecanumGuidanceStatus.msg
 	RoverMecanumSetpoint.msg
 	RoverMecanumStatus.msg

src/lib/rover_control/RoverControl.cpp

@@ -48,26 +48,24 @@ float throttleControl(SlewRate<float> &motor_setpoint, const float throttle_setp
 
 	if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate
 		motor_setpoint.setSlewRate(max_accel / max_thr_spd);
+		motor_setpoint.update(throttle_setpoint, dt);
 
 		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
 		if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint -
 				current_motor_setpoint)) {
-			motor_setpoint.setForcedValue(current_motor_setpoint);
+			motor_setpoint.setForcedValue(throttle_setpoint);
 		}
 
-		motor_setpoint.update(throttle_setpoint, dt);
-
 	} else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate
 		motor_setpoint.setSlewRate(max_decel / max_thr_spd);
+		motor_setpoint.update(throttle_setpoint, dt);
 
 		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
 		if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint -
 				current_motor_setpoint)) {
-			motor_setpoint.setForcedValue(current_motor_setpoint);
+			motor_setpoint.setForcedValue(throttle_setpoint);
 		}
 
-		motor_setpoint.update(throttle_setpoint, dt);
-
 	} else { // Fallthrough if slew rate parameters are not configured
 		motor_setpoint.setForcedValue(throttle_setpoint);
 	}
@@ -88,14 +86,13 @@ float attitudeControl(SlewRateYaw<float> &adjusted_yaw_setpoint, PID &pid_yaw,
 
 	if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured
 		adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate);
+		adjusted_yaw_setpoint.update(yaw_setpoint, dt);
 
 		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
 		if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) {
-			adjusted_yaw_setpoint.setForcedValue(vehicle_yaw);
+			adjusted_yaw_setpoint.setForcedValue(yaw_setpoint);
 		}
 
-		adjusted_yaw_setpoint.update(yaw_setpoint, dt);
-
 	} else {
 		adjusted_yaw_setpoint.setForcedValue(yaw_setpoint);
 	}
@@ -118,26 +115,24 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const
 	// Apply acceleration and deceleration limit
 	if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) {
 		speed_with_rate_limit.setSlewRate(max_accel);
+		speed_with_rate_limit.update(speed_setpoint, dt);
 
 		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
 		if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf(
 			    speed_setpoint - vehicle_speed)) {
-			speed_with_rate_limit.setForcedValue(vehicle_speed);
+			speed_with_rate_limit.setForcedValue(speed_setpoint);
 		}
 
-		speed_with_rate_limit.update(speed_setpoint, dt);
-
 	} else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) {
 		speed_with_rate_limit.setSlewRate(max_decel);
+		speed_with_rate_limit.update(speed_setpoint, dt);
 
 		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
 		if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf(
 			    speed_setpoint - vehicle_speed)) {
-			speed_with_rate_limit.setForcedValue(vehicle_speed);
+			speed_with_rate_limit.setForcedValue(speed_setpoint);
 		}
 
-		speed_with_rate_limit.update(speed_setpoint, dt);
-
 	} else { // Fallthrough if slew rate is not configured
 		speed_with_rate_limit.setForcedValue(speed_setpoint);
 	}
@@ -153,12 +148,72 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const
 	}
 
 	// Feedback control
-	pid_speed.setSetpoint(speed_with_rate_limit.getState());
-	forward_speed_normalized += pid_speed.update(vehicle_speed, dt);
+	if (fabsf(speed_with_rate_limit.getState()) > FLT_EPSILON) {
+		pid_speed.setSetpoint(speed_with_rate_limit.getState());
+		forward_speed_normalized += pid_speed.update(vehicle_speed, dt);
+
+	} else {
+		pid_speed.resetIntegral();
+	}
+
 
 	return math::constrain(forward_speed_normalized, -1.f, 1.f);
 }
 
+float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint,
+		  const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel,
+		  const float wheel_track, const float dt)
+{
+	// Apply acceleration and deceleration limit
+	if (fabsf(yaw_rate_setpoint) >= fabsf(vehicle_yaw_rate) && max_yaw_accel > FLT_EPSILON) {
+		adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_accel);
+		adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt);
+
+		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
+		if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf(
+			    yaw_rate_setpoint - vehicle_yaw_rate)) {
+			adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint);
+		}
+
+
+	} else if (fabsf(yaw_rate_setpoint) < fabsf(vehicle_yaw_rate) && max_yaw_decel > FLT_EPSILON) {
+		adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_decel);
+		adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt);
+
+		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
+		if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf(
+			    yaw_rate_setpoint - vehicle_yaw_rate)) {
+			adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint);
+		}
+
+
+	} else { // Fallthrough if slew rate is not configured
+		adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint);
+	}
+
+	// Transform yaw rate into speed difference
+	float speed_diff_normalized{0.f};
+
+	if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
+		const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track /
+					 2.f;
+		speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_yaw_r,
+					max_thr_yaw_r, -1.f, 1.f);
+	}
+
+	// Feedback control
+	if (fabsf(adjusted_yaw_rate_setpoint.getState()) > FLT_EPSILON) {
+		pid_yaw_rate.setSetpoint(adjusted_yaw_rate_setpoint.getState());
+		speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt);
+
+	} else {
+		pid_yaw_rate.resetIntegral();
+	}
+
+
+	return math::constrain(speed_diff_normalized, -1.f, 1.f);
+}
+
 void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
 				  position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
 				  MapProjection &global_ned_proj_ref)

src/lib/rover_control/RoverControl.hpp

@@ -92,6 +92,25 @@ float attitudeControl(SlewRateYaw<float> &adjusted_yaw_setpoint, PID &pid_yaw, f
 float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float speed_setpoint,
 		   float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt);
 
+/**
+ * Applies yaw acceleration slew rate to a yaw rate setpoint and calculates the necessary speed diff setpoint
+ * using a feedforward term and/or a PID controller.
+ * Note: This function is only for rovers that control the rate through a speed difference between the left/right wheels.
+ * @param adjusted_yaw_rate_setpoint Yaw rate setpoint with applied slew rate [-1, 1] (Updated by this function).
+ * @param pid_yaw_rate Yaw rate PID (Updated by this function).
+ * @param yaw_rate_setpoint Yaw rate setpoint [rad/s].
+ * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s].
+ * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s].
+ * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2].
+ * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2].
+ * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m].
+ * @param dt Time since last update [s].
+ * @return Normalized speed difference setpoint [-1, 1].
+ */
+float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate,
+		  float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel,
+		  float wheel_track, float dt);
+
 /**
  * Projects positionSetpointTriplet waypoints from global to ned frame.
  * @param curr_wp_ned Current waypoint in NED frame (Updated by this function)

src/lib/rover_control/module.yaml

@@ -133,7 +133,23 @@ parameters:
         RO_YAW_ACCEL_LIM:
           description:
             short: Yaw acceleration limit
-            long: Set to -1 to disable.
+            long: |
+              Used to cap how quickly the magnitude of yaw rate setpoints can increase.
+              Set to -1 to disable.
+          type: float
+          unit: deg/s^2
+          min: -1
+          max: 10000
+          increment: 0.01
+          decimal: 2
+          default: -1
+
+        RO_YAW_DECEL_LIM:
+          description:
+            short: Yaw deceleration limit
+            long: |
+              Used to cap how quickly the magnitude of yaw rate setpoints can decrease.
+              Set to -1 to disable.
           type: float
           unit: deg/s^2
           min: -1

src/modules/logger/logged_topics.cpp

@@ -110,9 +110,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("rover_rate_status", 100);
 	add_optional_topic("rover_steering_setpoint", 100);
 	add_optional_topic("rover_throttle_setpoint", 100);
-	add_optional_topic("rover_differential_guidance_status", 100);
-	add_optional_topic("rover_differential_setpoint", 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);

src/modules/rover_ackermann/Kconfig

@@ -2,4 +2,4 @@ menuconfig MODULES_ROVER_ACKERMANN
 	bool "rover_ackermann"
 	default n
 	---help---
-		Enable support for rover_ackermann
+		Enable support for ackermann rovers