Rover: Fixed saturation

PX4 · Dec 10, 2023 · 806b848 · 806b848
1 parent df458d9
commit 806b848
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 8 deletions.
diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.cpp b/src/modules/differential_drive_control/DifferentialDriveControl.cpp
@@ -190,10 +190,7 @@ void DifferentialDriveControl::Run()
 	const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
 	const bool vlid_max_speed = _param_rdd_max_speed.get() > FLT_EPSILON;
 
-	if (_armed && !setpoint_timeout && vlid_max_speed) {
-		wheel_speeds *= _param_rdd_wheel_radius.get() / _param_rdd_max_speed.get();
-
-	} else {
+	if (!_armed || setpoint_timeout || !vlid_max_speed) {
 		wheel_speeds = {}; // stop
 	}
 

diff --git a/...es/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/...es/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp
@@ -56,7 +56,9 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin
 	float gain = 1.0f;
 
 	if (combined_velocity > _max_speed) {
-		gain = (combined_velocity - _max_speed) / linear_velocity_x;
+		float excess_velocity = fabsf(combined_velocity - _max_speed);
+		float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
+		gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
 	}
 
 	// Apply the gain

diff --git a/...ifferential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/...ifferential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp
@@ -85,7 +85,7 @@ TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
 	kinematics.setMaxAngularVelocity(10.f);
 
 	// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
-	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(1.f / 6.f, 7.f / 6.f));
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
 }
 
 
@@ -98,7 +98,19 @@ TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
 	kinematics.setMaxAngularVelocity(10.f);
 
 	// Negative linear velocity for backward motion and positive yaw rate for turning right
-	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-7.f / 6.f, -1.f / 6.f));
+	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
+}
+
+TEST(DifferentialDriveKinematicsTest, RandomCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setWheelRadius(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
 }
 
 TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
@@ -146,7 +158,31 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
 	kinematics.setMaxAngularVelocity(1.f);
 
 	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
-	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.45238f, 1.45238f));
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setWheelRadius(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setWheelRadius(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
 }