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

sensors/vehicle_imu: pause computing publication update rate

This commit is contained in:
Daniel Agar 2020-06-06 11:46:01 -04:00
parent 786733cd1a
commit 39a33a35ae
2 changed files with 26 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -103,6 +103,8 @@ void VehicleIMU::ParametersUpdate(bool force)
parameter_update_s param_update;
_params_sub.copy(&param_update);
const auto imu_integ_rate_prev = _param_imu_integ_rate.get();
updateParams();
_accel_corrections.ParametersUpdate();
@ -115,6 +117,13 @@ void VehicleIMU::ParametersUpdate(bool force)
_param_imu_integ_rate.set(imu_integration_rate_hz);
_param_imu_integ_rate.commit_no_notification();
}
if (_param_imu_integ_rate.get() != imu_integ_rate_prev) {
// force update
_intervals_update = true;
_accel_interval.timestamp_sample_last = 0;
_gyro_interval.timestamp_sample_last = 0;
}
}
}
@ -144,6 +153,11 @@ bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstim
intavg.interval_sum = 0.f;
intavg.interval_count = 0.f;
}
} else {
// reset
intavg.interval_sum = 0.f;
intavg.interval_count = 0.f;
}
intavg.timestamp_sample_last = timestamp_sample;
@ -169,6 +183,10 @@ void VehicleIMU::Run()
if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) {
perf_count(_gyro_generation_gap_perf);
// if there's a gap in data start monitoring publication interval again
_intervals_update = true;
_gyro_interval.timestamp_sample_last = 0;
}
_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
@ -184,7 +202,7 @@ void VehicleIMU::Run()
_last_timestamp_sample_gyro = gyro.timestamp_sample;
// collect sample interval average for filters
if (UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
if (_intervals_update && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
update_integrator_config = true;
publish_status = true;
_status.gyro_rate_hz = roundf(1e6f / _gyro_interval.update_interval);
@ -203,6 +221,10 @@ void VehicleIMU::Run()
if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) {
perf_count(_accel_generation_gap_perf);
// if there's a gap in data start monitoring publication interval again
_intervals_update = true;
_accel_interval.timestamp_sample_last = 0;
}
_accel_last_generation = _sensor_accel_sub.get_last_generation();
@ -218,7 +240,7 @@ void VehicleIMU::Run()
_last_timestamp_sample_accel = accel.timestamp_sample;
// collect sample interval average for filters
if (UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
if (_intervals_update && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
update_integrator_config = true;
publish_status = true;
_status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
@ -351,6 +373,7 @@ void VehicleIMU::UpdateIntegratorConfiguration()
_sensor_accel_sub.unregisterCallback();
_intervals_configured = true;
_intervals_update = false; // stop monitoring topic publication rate
PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f, gyro interval: %.1f",
_accel_corrections.get_device_id(), _gyro_corrections.get_device_id(), accel_integral_samples, gyro_integral_samples,

1
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -113,6 +113,7 @@ private:
uint8_t _delta_velocity_clipping{0};
bool _intervals_update{true};
bool _intervals_configured{false};
perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")};