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

vehicle_imu: only reset raw accel/gyro Welford mean periodically

Browse Source 
- vehicle_imu_status can publish immediately on any measured sample
rate change or sensor error increment, but the windowed mean/variance
shouldn't necessarily reset that often
This commit is contained in:
Daniel Agar
2022-09-29 14:14:16 -04:00
parent f9522e831c
commit d4e5876d4c
2 changed files with 15 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+13 -5
View File
@@ -620,15 +620,18 @@ bool VehicleIMU::Publish()
// vehicle_imu_status // vehicle_imu_status
// publish before vehicle_imu so that error counts are available synchronously if needed // publish before vehicle_imu so that error counts are available synchronously if needed
if (_raw_accel_mean.valid() && _raw_gyro_mean.valid() const bool imu_status_publishing_interval_exceeded = hrt_elapsed_time(&_status.timestamp) >=
&& (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) { kIMUStatusPublishingInterval;
if (_raw_accel_mean.valid() && _raw_gyro_mean.valid()
&& (_publish_status || imu_status_publishing_interval_exceeded)) {
// Accel
_status.accel_device_id = _accel_calibration.device_id(); _status.accel_device_id = _accel_calibration.device_id();
// accel mean and variance // accel mean and variance
Vector3f(_accel_calibration.rotation() * _raw_accel_mean.mean()).copyTo(_status.mean_accel); Vector3f(_accel_calibration.rotation() * _raw_accel_mean.mean()).copyTo(_status.mean_accel);
Vector3f(_accel_calibration.rotation() * _raw_accel_mean.variance()).copyTo(_status.var_accel); Vector3f(_accel_calibration.rotation() * _raw_accel_mean.variance()).copyTo(_status.var_accel);
_raw_accel_mean.reset();
// accel temperature // accel temperature
_status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count; _status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count;
@@ -636,12 +639,12 @@ bool VehicleIMU::Publish()
_accel_temperature_sum_count = 0; _accel_temperature_sum_count = 0;
// Gyro
_status.gyro_device_id = _gyro_calibration.device_id(); _status.gyro_device_id = _gyro_calibration.device_id();
// gyro mean and variance // gyro mean and variance
Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro); Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro);
Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.variance()).copyTo(_status.var_gyro); Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.variance()).copyTo(_status.var_gyro);
_raw_gyro_mean.reset();
// Gyro delta angle coning metric = length of coning corrections averaged since last status publication // Gyro delta angle coning metric = length of coning corrections averaged since last status publication
_status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s; _status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s;
@@ -653,11 +656,16 @@ bool VehicleIMU::Publish()
_gyro_temperature_sum = NAN; _gyro_temperature_sum = NAN;
_gyro_temperature_sum_count = 0; _gyro_temperature_sum_count = 0;
// publish
_status.timestamp = hrt_absolute_time(); _status.timestamp = hrt_absolute_time();
_vehicle_imu_status_pub.publish(_status); _vehicle_imu_status_pub.publish(_status);
_publish_status = false; _publish_status = false;
if (imu_status_publishing_interval_exceeded) {
_raw_accel_mean.reset();
_raw_gyro_mean.reset();
}
} }
// publish vehicle_imu // publish vehicle_imu
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+2
View File
@@ -95,6 +95,8 @@ private:
uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
static constexpr hrt_abstime kIMUStatusPublishingInterval{100_ms};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
// Used to check, save and use learned magnetometer biases // Used to check, save and use learned magnetometer biases