dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_angular_velocity: add IMU_GYRO_RATEMAX constraints

This commit is contained in:
Daniel Agar 2022-06-04 11:29:38 -04:00
parent beaebe4d0b
commit 6823cbc414
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
1 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -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) {