sensors/vehicle_angular_velocity: add IMU_GYRO_RATEMAX constraints

PX4 · Jun 7, 2022 · 6823cbc · 6823cbc
1 parent beaebe4
commit 6823cbc
Showing 1 changed file with 17 additions and 0 deletions.
diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
@@ -377,6 +377,23 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 
 		_calibration.ParametersUpdate();
 
+		// IMU_GYRO_RATEMAX
+		if (_param_imu_gyro_ratemax.get() <= 0) {
+			const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();
+			_param_imu_gyro_ratemax.reset();
+			PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax,
+				 _param_imu_gyro_ratemax.get());
+		}
+
+		// constrain IMU_GYRO_RATEMAX 50-10,000 Hz
+		const int32_t imu_gyro_ratemax = constrain(_param_imu_gyro_ratemax.get(), (int32_t)50, (int32_t)10'000);
+
+		if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) {
+			PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax);
+			_param_imu_gyro_ratemax.set(imu_gyro_ratemax);
+			_param_imu_gyro_ratemax.commit_no_notification();
+		}
+
 		// gyro low pass cutoff frequency changed
 		for (auto &lp : _lp_filter_velocity) {
 			if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) {