rover: constrain update steps

chfriedrich98 · chfriedrich98 · commit 4a5eabb61e3c · 2025-08-22T12:13:01.000+02:00
diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp
@@ -57,7 +57,7 @@ void AckermannActControl::updateActControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	// Motor control
 	if (_rover_throttle_setpoint_sub.updated()) {
diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp
@@ -65,7 +65,7 @@ void AckermannAttControl::updateAttControl()
 
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	if (PX4_ISFINITE(_yaw_setpoint)) {
 		// Calculate yaw rate limit for slew rate
diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp
@@ -62,7 +62,7 @@ void AckermannRateControl::updateRateControl()
 
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	if (PX4_ISFINITE(_yaw_rate_setpoint)) {
 		if (fabsf(_estimated_speed) > FLT_EPSILON) {
diff --git a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp
@@ -63,7 +63,7 @@ void AckermannSpeedControl::updateSpeedControl()
 
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	// Throttle Setpoint
 	if (PX4_ISFINITE(_speed_setpoint)) {
diff --git a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp
@@ -113,7 +113,7 @@ class AckermannSpeedControl : public ModuleParams
 
 	// Controllers
 	PID _pid_speed;
-	SlewRate<float> _adjusted_speed_setpoint;
+	SlewRate<float> _adjusted_speed_setpoint{0.f};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp
@@ -53,7 +53,7 @@ void DifferentialActControl::updateActControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	// Motor control
 	if (_rover_throttle_setpoint_sub.updated()) {
diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp
@@ -63,7 +63,7 @@ void DifferentialAttControl::updateAttControl()
 {
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp
@@ -59,7 +59,7 @@ void DifferentialRateControl::updateRateControl()
 {
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	if (_vehicle_angular_velocity_sub.updated()) {
 		vehicle_angular_velocity_s vehicle_angular_velocity{};
diff --git a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp
@@ -63,7 +63,7 @@ void DifferentialSpeedControl::updateSpeedControl()
 
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	// Throttle Setpoint
 	if (PX4_ISFINITE(_speed_setpoint)) {
diff --git a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp
@@ -122,7 +122,7 @@ class DifferentialSpeedControl : public ModuleParams
 
 	// Controllers
 	PID _pid_speed;
-	SlewRate<float> _adjusted_speed_setpoint;
+	SlewRate<float> _adjusted_speed_setpoint{0.f};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp
@@ -54,7 +54,7 @@ void MecanumActControl::updateActControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	// Motor control
 	if (_rover_throttle_setpoint_sub.updated()) {
diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp
@@ -63,7 +63,7 @@ void MecanumAttControl::updateAttControl()
 {
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp
@@ -59,7 +59,7 @@ void MecanumRateControl::updateRateControl()
 {
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	if (_vehicle_angular_velocity_sub.updated()) {
 		vehicle_angular_velocity_s vehicle_angular_velocity{};
diff --git a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp
@@ -65,7 +65,7 @@ void MecanumSpeedControl::updateSpeedControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;
 
 	updateSubscriptions();
 
diff --git a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp
@@ -126,8 +126,8 @@ class MecanumSpeedControl : public ModuleParams
 	// Controllers
 	PID _pid_speed_x;
 	PID _pid_speed_y;
-	SlewRate<float> _adjusted_speed_x_setpoint;
-	SlewRate<float> _adjusted_speed_y_setpoint;
+	SlewRate<float> _adjusted_speed_x_setpoint{0.f};
+	SlewRate<float> _adjusted_speed_y_setpoint{0.f};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,

Original file line number	Diff line number	Diff line change
`@@ -57,7 +57,7 @@ void AckermannActControl::updateActControl()`
`57`	`57`	`{`
`58`	`58`	`const hrt_abstime timestamp_prev = _timestamp;`
`59`	`59`	`_timestamp = hrt_absolute_time();`
`60`		`- const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;`
	`60`	`+ const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;`
`61`	`61`
`62`	`62`	`// Motor control`
`63`	`63`	`if (_rover_throttle_setpoint_sub.updated()) {`
Original file line number	Diff line number	Diff line change
`@@ -53,7 +53,7 @@ void DifferentialActControl::updateActControl()`
`53`	`53`	`{`
`54`	`54`	`const hrt_abstime timestamp_prev = _timestamp;`
`55`	`55`	`_timestamp = hrt_absolute_time();`
`56`		`- const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;`
	`56`	`+ const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;`
`57`	`57`
`58`	`58`	`// Motor control`
`59`	`59`	`if (_rover_throttle_setpoint_sub.updated()) {`
Original file line number	Diff line number	Diff line change
`@@ -63,7 +63,7 @@ void DifferentialAttControl::updateAttControl()`
`63`	`63`	`{`
`64`	`64`	`hrt_abstime timestamp_prev = _timestamp;`
`65`	`65`	`_timestamp = hrt_absolute_time();`
`66`		`- const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;`
	`66`	`+ const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;`
`67`	`67`
`68`	`68`	`if (_vehicle_attitude_sub.updated()) {`
`69`	`69`	`vehicle_attitude_s vehicle_attitude{};`
Original file line number	Diff line number	Diff line change
`@@ -59,7 +59,7 @@ void DifferentialRateControl::updateRateControl()`
`59`	`59`	`{`
`60`	`60`	`hrt_abstime timestamp_prev = _timestamp;`
`61`	`61`	`_timestamp = hrt_absolute_time();`
`62`		`- const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;`
	`62`	`+ const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f;`
`63`	`63`
`64`	`64`	`if (_vehicle_angular_velocity_sub.updated()) {`
`65`	`65`	`vehicle_angular_velocity_s vehicle_angular_velocity{};`