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

vehicle_imu: only reset raw accel/gyro Welford mean periodically 

- 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-18 11:25:24 -04:00
parent e3d730ea37
commit 6ce38b8296
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
2 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -574,15 +574,18 @@ bool VehicleIMU::Publish()
// vehicle_imu_status
// publish before vehicle_imu so that error counts are available synchronously if needed
if (_raw_accel_mean.valid() && _raw_gyro_mean.valid()
&& (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) {
const bool imu_status_publishing_interval_exceeded = hrt_elapsed_time(&_status.timestamp) >=
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();
// accel mean and variance
Vector3f(_accel_calibration.rotation() * _raw_accel_mean.mean()).copyTo(_status.mean_accel);
Vector3f(_accel_calibration.rotation() * _raw_accel_mean.variance()).copyTo(_status.var_accel);
_raw_accel_mean.reset();
// accel temperature
_status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count;
@ -590,12 +593,12 @@ bool VehicleIMU::Publish()
_accel_temperature_sum_count = 0;
// Gyro
_status.gyro_device_id = _gyro_calibration.device_id();
// gyro mean and variance
Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.mean()).copyTo(_status.mean_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
_status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s;
@ -607,11 +610,16 @@ bool VehicleIMU::Publish()
_gyro_temperature_sum = NAN;
_gyro_temperature_sum_count = 0;
// publish
_status.timestamp = hrt_absolute_time();
_vehicle_imu_status_pub.publish(_status);
_publish_status = false;
if (imu_status_publishing_interval_exceeded) {
_raw_accel_mean.reset();
_raw_gyro_mean.reset();
}
}
// publish vehicle_imu

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

@ -95,6 +95,8 @@ private:
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)};
static constexpr hrt_abstime kIMUStatusPublishingInterval{100_ms};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
// Used to check, save and use learned magnetometer biases