fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 20:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_acceleration: require valid sample rate before running

Browse Source 
- this fixes a minor edge case when you only have a single accelerometer and haven't made any parameter changes, the sensor sample rate (used for filtering) might not have been updated from default
This commit is contained in:
Daniel Agar
2021-05-24 21:39:42 -04:00
committed by GitHub
parent 22838f491a
commit b264577daa
2 changed files with 11 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+10 -1
View File
@@ -94,7 +94,7 @@ void VehicleAcceleration::CheckAndUpdateFilters()
if ((imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id())
&& PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
// check if sample rate error is greater than 1%
if ((fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
_filter_sample_rate = sample_rate_hz;
sample_rate_changed = true;
@@ -214,6 +214,15 @@ void VehicleAcceleration::Run()
SensorBiasUpdate(selection_updated);
ParametersUpdate();
// require valid sensor sample rate to run
if (!PX4_ISFINITE(_filter_sample_rate)) {
CheckAndUpdateFilters();
if (!PX4_ISFINITE(_filter_sample_rate)) {
return;
}
}
// process all outstanding messages
sensor_accel_s sensor_data;
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+1 -1
View File
@@ -94,7 +94,7 @@ private:
matrix::Vector3f _acceleration_prev{};
static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
float _filter_sample_rate{kInitialRateHz};
float _filter_sample_rate{NAN};
math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.f};