fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 22:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

create Welford mean Vector with covariance and improve precision with Kahan summation (#20676)

Browse Source 
- WelfordMeanVector now computes covariance
 - use Kahan summation for Welford mean (but continue using float32 for actual mean, etc)
 - WelfordMean and WelfordMeanVector handle initial value and count roll over
 - Welford mean count rollover at 16 bit max to prevent numerical issues and shift weight to newer samples
 - sensors/vehicle_imu: update Welford mean usage (now simplified with resets removed)
    - fix vehicle_imu_status accel var, now properly rotated with full covariance matrix
 - gyro_calibration: update Welford mean usage
This commit is contained in:
Daniel Agar
2022-11-30 14:50:13 -05:00
committed by GitHub
parent 5155346d60
commit 89b81b0bd6
9 changed files with 527 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gyro_calibration/GyroCalibration.cpp
+43 -40
View File
@@ -130,7 +130,7 @@ void GyroCalibration::Run()
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
sensor_gyro_s sensor_gyro;
while (_sensor_gyro_subs[gyro].update(&sensor_gyro)) {
if (_sensor_gyro_subs[gyro].update(&sensor_gyro)) {
if (PX4_ISFINITE(sensor_gyro.temperature)) {
if ((fabsf(_temperature[gyro] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[gyro])) {
PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", gyro, (double)_temperature[gyro],
@@ -140,6 +140,7 @@ void GyroCalibration::Run()
// reset all on any temperature change
Reset();
return;
}
} else {
@@ -156,13 +157,16 @@ void GyroCalibration::Run()
// setting device id, reset all
_gyro_calibration[gyro].set_device_id(sensor_gyro.device_id);
Reset();
return;
}
}
if ((_gyro_last_update[gyro] != 0) && (hrt_elapsed_time(&_gyro_last_update[gyro]) > 100_ms)) {
// reset on any timeout
// remove sensor and reset on any timeout
_gyro_calibration[gyro].set_device_id(0);
_gyro_calibration[gyro].Reset();
Reset();
_gyro_last_update[gyro] = 0;
return;
}
}
@@ -190,19 +194,14 @@ void GyroCalibration::Run()
}
}
// update calibrations for all available gyros
if (hrt_elapsed_time(&_last_calibration_update) > 5_s) {
// check if sufficient data has been gathered to update calibration
bool sufficient_samples = false;
// check variance again before saving
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0) {
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if ((_gyro_calibration[gyro].device_id() != 0) && _gyro_mean[gyro].valid()) {
// periodically check variance
if (_gyro_mean[gyro].count() % 100 == 0) {
PX4_DEBUG("gyro %d (%" PRIu32 ") variance, [%.9f, %.9f, %.9f] %.9f", gyro, _gyro_calibration[gyro].device_id(),
(double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2),
(double)_gyro_mean[gyro].variance().length());
if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
if (!_gyro_mean[gyro].valid() || _gyro_mean[gyro].variance().longerThan(0.001f)) {
// reset all
PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
gyro, (double)_gyro_mean[gyro].variance().length());
@@ -210,44 +209,49 @@ void GyroCalibration::Run()
return;
}
}
if (_gyro_mean[gyro].count() > 5000) {
sufficient_samples = true;
} else {
sufficient_samples = false;
return;
}
}
}
// update calibrations for all available gyros
if (sufficient_samples && (hrt_elapsed_time(&_last_calibration_update) > 10_s)) {
bool calibration_updated = false;
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0 && _gyro_mean[gyro].valid()) {
// check variance again before saving
if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
// reset all
PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
gyro, (double)_gyro_mean[gyro].variance().length());
Reset();
return;
}
if ((_gyro_calibration[gyro].device_id() != 0)
&& _gyro_mean[gyro].valid() && (_gyro_mean[gyro].count() > 100)
) {
const Vector3f old_offset{_gyro_calibration[gyro].offset()};
const Vector3f new_offset{_gyro_mean[gyro].mean()};
if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean()) || !_gyro_calibration[gyro].calibrated()) {
bool change_exceeds_stddev = false;
bool variance_significantly_better = false;
const Vector3f variance = _gyro_mean[gyro].variance();
for (int i = 0; i < 3; i++) {
// check if offset changed by more than 1 standard deviation
if (sq(new_offset(i) - old_offset(i)) > variance(i)) {
change_exceeds_stddev = true;
}
// check if current variance is significantly better than previous cal
if (variance(i) < 0.1f * _gyro_cal_variance[gyro](i)) {
variance_significantly_better = true;
}
}
// update if offset changed by more than 1 standard deviation or currently uncalibrated
if ((change_exceeds_stddev || variance_significantly_better || !_gyro_calibration[gyro].calibrated())
&& _gyro_calibration[gyro].set_offset(new_offset)
) {
calibration_updated = true;
_gyro_cal_variance[gyro] = variance;
PX4_INFO("gyro %d (%" PRIu32 ") updating offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] %.1f degC",
gyro, _gyro_calibration[gyro].device_id(),
(double)old_offset(0), (double)old_offset(1), (double)old_offset(2),
(double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2),
(double)new_offset(0), (double)new_offset(1), (double)new_offset(2),
(double)_temperature[gyro]);
perf_count(_calibration_updated_perf);
@@ -269,11 +273,10 @@ void GyroCalibration::Run()
if (param_save) {
param_notify_changes();
_last_calibration_update = hrt_absolute_time();
}
}
Reset();
Reset();
}
}
}
src/modules/gyro_calibration/GyroCalibration.hpp
+11 -4
View File
@@ -39,7 +39,7 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/math/WelfordMean.hpp>
#include <lib/mathlib/math/WelfordMeanVector.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <uORB/Subscription.hpp>
@@ -79,11 +79,17 @@ private:
void Reset()
{
for (auto &m : _gyro_mean) {
m.reset();
for (int gyro = 0; gyro < MAX_SENSORS; gyro++) {
_gyro_mean[gyro].reset();
_gyro_last_update[gyro] = 0;
}
_last_calibration_update = hrt_absolute_time();
}
// return the square of two floating point numbers
static constexpr float sq(float var) { return var * var; }
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status};
@@ -91,7 +97,8 @@ private:
uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS> _sensor_gyro_subs{ORB_ID::sensor_gyro};
calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
math::WelfordMean<float, 3> _gyro_mean[MAX_SENSORS] {};
math::WelfordMeanVector<float, 3> _gyro_mean[MAX_SENSORS] {};
matrix::Vector3f _gyro_cal_variance[MAX_SENSORS] {};
float _temperature[MAX_SENSORS] {};
hrt_abstime _gyro_last_update[MAX_SENSORS] {};
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+155 -167
View File
@@ -198,12 +198,15 @@ void VehicleIMU::Run()
bool consume_all_gyro = !_intervals_configured || _data_gap;
// monitor scheduling latency and force catch up with latest gyro if falling behind
if (_sensor_gyro_sub.updated() && (_gyro_update_latency_mean.count() > 100)
&& (_gyro_update_latency_mean.mean()(1) > _gyro_interval_us * 1e-6f)) {
PX4_DEBUG("gyro update mean sample latency: %.6f, publish latency %.6f",
(double)_gyro_update_latency_mean.mean()(0),
(double)_gyro_update_latency_mean.mean()(1));
if (_intervals_configured && !consume_all_gyro && _sensor_gyro_sub.updated()
&& _gyro_publish_latency_mean_us.valid() && _gyro_mean_interval_us.valid()
&& (_gyro_publish_latency_mean_us.mean() > _gyro_mean_interval_us.mean())
) {
PX4_DEBUG("%d gyro publish latency %.6f > %.6f us (mean interval)",
_instance,
(double)_gyro_publish_latency_mean_us.mean(),
(double)_gyro_mean_interval_us.mean()
);
consume_all_gyro = true;
}
@@ -219,7 +222,8 @@ void VehicleIMU::Run()
// update accel until integrator ready and caught up to gyro
while (_sensor_accel_sub.updated()
&& (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap
|| (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)))) {
|| (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)))
) {
if (UpdateAccel()) {
updated = true;
@@ -239,16 +243,6 @@ void VehicleIMU::Run()
// publish if both accel & gyro integrators are ready
if (_intervals_configured && _accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) {
if (Publish()) {
// record gyro publication latency and integrated samples
if (_gyro_update_latency_mean.count() > 10000) {
// reset periodically to avoid numerical issues
_gyro_update_latency_mean.reset();
}
const float time_run_s = now_us * 1e-6f;
_gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f});
break;
}
}
@@ -285,15 +279,28 @@ bool VehicleIMU::UpdateAccel()
_data_gap = true;
perf_count(_accel_generation_gap_perf);
// reset average sample measurement
_accel_interval_mean.reset();
} else {
// collect sample interval average for filters
if (accel.timestamp_sample > _accel_timestamp_sample_last) {
if ((_accel_timestamp_sample_last != 0) && (accel.samples > 0)) {
float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last;
_accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples});
if (_accel_timestamp_sample_last != 0) {
const float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last;
_accel_mean_interval_us.update(interval_us);
_accel_fifo_mean_interval_us.update(interval_us / math::max(accel.samples, (uint8_t)1));
// check measured interval periodically
if (_accel_mean_interval_us.valid() && (_accel_mean_interval_us.count() % 10 == 0)) {
const float interval_mean = _accel_mean_interval_us.mean();
// update sample rate if previously invalid or changed by more than 1 standard deviation
const bool diff_exceeds_stddev = sq(interval_mean - _accel_interval_us) > _accel_mean_interval_us.variance();
if (!PX4_ISFINITE(_accel_interval_us) || diff_exceeds_stddev) {
// update integrator configuration if interval has changed by more than 10%
_update_integrator_config = true;
}
}
}
} else {
@@ -305,47 +312,6 @@ bool VehicleIMU::UpdateAccel()
PX4_ERR("%d - accel %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")",
_instance, accel.device_id, accel.timestamp, accel.timestamp_sample);
}
const int interval_count = _accel_interval_mean.count();
const float interval_variance = _accel_interval_mean.variance()(0);
// check measured interval periodically
if ((_accel_interval_mean.valid() && (interval_count % 10 == 0))
&& (!PX4_ISFINITE(_accel_interval_best_variance)
|| (interval_variance < _accel_interval_best_variance)
|| (interval_count > 1000))) {
const float interval_mean = _accel_interval_mean.mean()(0);
const float interval_mean_fifo = _accel_interval_mean.mean()(1);
// update sample rate if previously invalid or changed
const float interval_delta_us = fabsf(interval_mean - _accel_interval_us);
const float percent_changed = interval_delta_us / _accel_interval_us;
if (!PX4_ISFINITE(_accel_interval_us) || (percent_changed > 0.001f)) {
if (PX4_ISFINITE(interval_mean) && PX4_ISFINITE(interval_mean_fifo) && PX4_ISFINITE(interval_variance)) {
// update integrator configuration if interval has changed by more than 10%
if (interval_delta_us > 0.1f * _accel_interval_us) {
_update_integrator_config = true;
}
_accel_interval_us = interval_mean;
_accel_interval_best_variance = interval_variance;
_status.accel_rate_hz = 1e6f / interval_mean;
_status.accel_raw_rate_hz = 1e6f / interval_mean_fifo; // FIFO
_publish_status = true;
} else {
_accel_interval_mean.reset();
}
}
}
if (interval_count > 10000) {
// reset periodically to prevent numerical issues
_accel_interval_mean.reset();
}
}
_accel_last_generation = _sensor_accel_sub.get_last_generation();
@@ -359,15 +325,17 @@ bool VehicleIMU::UpdateAccel()
// temperature average
if (_accel_temperature_sum_count == 0) {
_accel_temperature_sum = accel.temperature;
if (PX4_ISFINITE(accel.temperature)) {
if ((_accel_temperature_sum_count == 0) || !PX4_ISFINITE(_accel_temperature_sum)) {
_accel_temperature_sum = accel.temperature;
_accel_temperature_sum_count = 1;
} else {
_accel_temperature_sum += accel.temperature;
} else {
_accel_temperature_sum += accel.temperature;
_accel_temperature_sum_count += 1;
}
}
_accel_temperature_sum_count++;
const float dt = (accel.timestamp_sample - _accel_timestamp_sample_last) * 1e-6f;
_accel_timestamp_sample_last = accel.timestamp_sample;
@@ -440,15 +408,28 @@ bool VehicleIMU::UpdateGyro()
_data_gap = true;
perf_count(_gyro_generation_gap_perf);
// reset average sample measurement
_gyro_interval_mean.reset();
} else {
// collect sample interval average for filters
if (gyro.timestamp_sample > _gyro_timestamp_sample_last) {
if ((_gyro_timestamp_sample_last != 0) && (gyro.samples > 0)) {
float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last;
_gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples});
if (_gyro_timestamp_sample_last != 0) {
const float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last;
_gyro_mean_interval_us.update(interval_us);
_gyro_fifo_mean_interval_us.update(interval_us / math::max(gyro.samples, (uint8_t)1));
// check measured interval periodically
if (_gyro_mean_interval_us.valid() && (_gyro_mean_interval_us.count() % 10 == 0)) {
const float interval_mean = _gyro_mean_interval_us.mean();
// update sample rate if previously invalid or changed by more than 1 standard deviation
const bool diff_exceeds_stddev = sq(interval_mean - _gyro_interval_us) > _gyro_mean_interval_us.variance();
if (!PX4_ISFINITE(_gyro_interval_us) || diff_exceeds_stddev) {
// update integrator configuration if interval has changed by more than 10%
_update_integrator_config = true;
}
}
}
} else {
@@ -460,50 +441,13 @@ bool VehicleIMU::UpdateGyro()
PX4_ERR("%d - gyro %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")",
_instance, gyro.device_id, gyro.timestamp, gyro.timestamp_sample);
}
const int interval_count = _gyro_interval_mean.count();
const float interval_variance = _gyro_interval_mean.variance()(0);
// check measured interval periodically
if ((_gyro_interval_mean.valid() && (interval_count % 10 == 0))
&& (!PX4_ISFINITE(_gyro_interval_best_variance)
|| (interval_variance < _gyro_interval_best_variance)
|| (interval_count > 1000))) {
const float interval_mean = _gyro_interval_mean.mean()(0);
const float interval_mean_fifo = _gyro_interval_mean.mean()(1);
// update sample rate if previously invalid or changed
const float interval_delta_us = fabsf(interval_mean - _gyro_interval_us);
const float percent_changed = interval_delta_us / _gyro_interval_us;
if (!PX4_ISFINITE(_gyro_interval_us) || (percent_changed > 0.001f)) {
if (PX4_ISFINITE(interval_mean) && PX4_ISFINITE(interval_mean_fifo) && PX4_ISFINITE(interval_variance)) {
// update integrator configuration if interval has changed by more than 10%
if (interval_delta_us > 0.1f * _gyro_interval_us) {
_update_integrator_config = true;
}
_gyro_interval_us = interval_mean;
_gyro_interval_best_variance = interval_variance;
_status.gyro_rate_hz = 1e6f / interval_mean;
_status.gyro_raw_rate_hz = 1e6f / interval_mean_fifo; // FIFO
_publish_status = true;
} else {
_gyro_interval_mean.reset();
}
}
}
if (interval_count > 10000) {
// reset periodically to prevent numerical issues
_gyro_interval_mean.reset();
}
}
_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f;
_gyro_timestamp_sample_last = gyro.timestamp_sample;
_gyro_timestamp_last = gyro.timestamp;
_gyro_calibration.set_device_id(gyro.device_id);
@@ -513,21 +457,18 @@ bool VehicleIMU::UpdateGyro()
_status.gyro_error_count = gyro.error_count;
}
// temperature average
if (_gyro_temperature_sum_count == 0) {
_gyro_temperature_sum = gyro.temperature;
if (PX4_ISFINITE(gyro.temperature)) {
if ((_gyro_temperature_sum_count == 0) || !PX4_ISFINITE(_gyro_temperature_sum)) {
_gyro_temperature_sum = gyro.temperature;
_gyro_temperature_sum_count = 1;
} else {
_gyro_temperature_sum += gyro.temperature;
} else {
_gyro_temperature_sum += gyro.temperature;
_gyro_temperature_sum_count += 1;
}
}
_gyro_temperature_sum_count++;
const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f;
_gyro_timestamp_sample_last = gyro.timestamp_sample;
const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z};
_raw_gyro_mean.update(gyro_raw);
_gyro_integrator.put(gyro_raw, dt);
@@ -620,41 +561,66 @@ bool VehicleIMU::Publish()
// vehicle_imu_status
// publish before vehicle_imu so that error counts are available synchronously if needed
const bool imu_status_publishing_interval_exceeded = hrt_elapsed_time(&_status.timestamp) >=
kIMUStatusPublishingInterval;
const bool status_publish_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_mean_interval_us.valid() && _gyro_mean_interval_us.valid()
&& (_publish_status || status_publish_interval_exceeded)
) {
// Accel
_status.accel_device_id = _accel_calibration.device_id();
{
_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);
_status.accel_rate_hz = 1e6f / _accel_mean_interval_us.mean();
_status.accel_raw_rate_hz = 1e6f / _accel_fifo_mean_interval_us.mean(); // FIFO
// accel temperature
_status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count;
_accel_temperature_sum = NAN;
_accel_temperature_sum_count = 0;
// accel mean and variance
const Dcmf &R = _accel_calibration.rotation();
Vector3f(R * _raw_accel_mean.mean()).copyTo(_status.mean_accel);
// variance from R * COV * R^T
const Matrix3f cov = R * _raw_accel_mean.covariance() * R.transpose();
cov.diag().copyTo(_status.var_accel);
// temperature
if ((_accel_temperature_sum_count > 0) && PX4_ISFINITE(_accel_temperature_sum)) {
_status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count;
} else {
_status.temperature_accel = NAN;
}
}
// Gyro
_status.gyro_device_id = _gyro_calibration.device_id();
{
_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);
_status.gyro_rate_hz = 1e6f / _gyro_mean_interval_us.mean();
_status.gyro_raw_rate_hz = 1e6f / _gyro_fifo_mean_interval_us.mean(); // FIFO
// 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;
_coning_norm_accum = 0;
_coning_norm_accum_total_time_s = 0;
// gyro mean and variance
const Dcmf &R = _gyro_calibration.rotation();
Vector3f(R * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro);
// gyro temperature
_status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count;
_gyro_temperature_sum = NAN;
_gyro_temperature_sum_count = 0;
// variance from R * COV * R^T
const Matrix3f cov = R * _raw_gyro_mean.covariance() * R.transpose();
cov.diag().copyTo(_status.var_gyro);
// 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;
_coning_norm_accum = 0;
_coning_norm_accum_total_time_s = 0;
// temperature
if ((_gyro_temperature_sum_count > 0) && PX4_ISFINITE(_gyro_temperature_sum)) {
_status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count;
} else {
_status.temperature_gyro = NAN;
}
}
// publish
_status.timestamp = hrt_absolute_time();
@@ -662,9 +628,14 @@ bool VehicleIMU::Publish()
_publish_status = false;
if (imu_status_publishing_interval_exceeded) {
if (status_publish_interval_exceeded) {
_raw_accel_mean.reset();
_accel_temperature_sum = NAN;
_accel_temperature_sum_count = 0;
_raw_gyro_mean.reset();
_gyro_temperature_sum = NAN;
_gyro_temperature_sum_count = 0;
}
}
@@ -683,6 +654,10 @@ bool VehicleIMU::Publish()
// reset clip counts
_delta_velocity_clipping = 0;
// record gyro publication latency and integrated samples
_gyro_publish_latency_mean_us.update(imu.timestamp - _gyro_timestamp_last);
_gyro_update_latency_mean_us.update(imu.timestamp - _gyro_timestamp_sample_last);
updated = true;
}
}
@@ -692,31 +667,34 @@ bool VehicleIMU::Publish()
void VehicleIMU::UpdateIntegratorConfiguration()
{
if (PX4_ISFINITE(_accel_interval_us) && PX4_ISFINITE(_gyro_interval_us)) {
if (_accel_mean_interval_us.valid() && _gyro_mean_interval_us.valid()) {
const float accel_interval_us = _accel_mean_interval_us.mean();
const float gyro_interval_us = _gyro_mean_interval_us.mean();
// determine number of sensor samples that will get closest to the desired integration interval
uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us));
uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / gyro_interval_us));
// if gyro samples exceeds queue depth, instead round to nearest even integer to improve scheduling options
if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) {
gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us / 2) * 2);
gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / gyro_interval_us / 2) * 2);
}
uint32_t integration_interval_us = roundf(gyro_integral_samples * _gyro_interval_us);
uint32_t integration_interval_us = roundf(gyro_integral_samples * gyro_interval_us);
// accel follows gyro as closely as possible
uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / _accel_interval_us));
uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / accel_interval_us));
// let the gyro set the configuration and scheduling
// relaxed minimum integration time required
_accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval_us));
_accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * accel_interval_us));
_accel_integrator.set_reset_samples(accel_integral_samples);
_gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us));
_gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * gyro_interval_us));
_gyro_integrator.set_reset_samples(gyro_integral_samples);
_backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us,
sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us) / 2, 1000, 20000);
_backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * accel_interval_us,
sensor_gyro_s::ORB_QUEUE_LENGTH * gyro_interval_us) / 2, 1000, 20000);
// gyro: find largest integer multiple of gyro_integral_samples
for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) {
@@ -734,7 +712,10 @@ void VehicleIMU::UpdateIntegratorConfiguration()
PX4_DEBUG("accel (%" PRIu32 "), gyro (%" PRIu32 "), accel samples: %" PRIu8 ", gyro samples: %" PRIu8
", accel interval: %.1f, gyro interval: %.1f sub samples: %d",
_accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples,
(double)_accel_interval_us, (double)_gyro_interval_us, n);
(double)accel_interval_us, (double)_gyro_interval_us, n);
_accel_interval_us = accel_interval_us;
_gyro_interval_us = gyro_interval_us;
break;
}
@@ -765,12 +746,19 @@ void VehicleIMU::PrintStatus()
PX4_INFO_RAW("[vehicle_imu] %" PRIu8 " - Accel: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro: %" PRIu32
", interval: %.1f us (SD %.1f us)\n",
_instance,
_accel_calibration.device_id(), (double)_accel_interval_us, (double)sqrtf(_accel_interval_best_variance),
_gyro_calibration.device_id(), (double)_gyro_interval_us, (double)sqrtf(_gyro_interval_best_variance));
_accel_calibration.device_id(), (double)_accel_mean_interval_us.mean(),
(double)_accel_mean_interval_us.standard_deviation(),
_gyro_calibration.device_id(), (double)_gyro_mean_interval_us.mean(),
(double)_gyro_mean_interval_us.standard_deviation());
PX4_DEBUG("gyro update mean sample latency: %.6f s, publish latency %.6f s, gyro interval %.6f s",
(double)_gyro_update_latency_mean.mean()(0), (double)_gyro_update_latency_mean.mean()(1),
(double)(_gyro_interval_us * 1e-6f));
#if defined(DEBUG_BUILD)
PX4_INFO_RAW("[vehicle_imu] %" PRIu8
" - gyro update sample latency: %.1f us (SD %.1f us), publish latency %.1f us (SD %.1f us)\n",
_instance,
(double)_gyro_update_latency_mean_us.mean(), (double)_gyro_update_latency_mean_us.standard_deviation(),
(double)_gyro_publish_latency_mean_us.mean(), (double)_gyro_publish_latency_mean_us.standard_deviation()
);
#endif // DEBUG_BUILD
perf_print_counter(_accel_generation_gap_perf);
perf_print_counter(_gyro_generation_gap_perf);
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+13 -8
View File
@@ -37,6 +37,7 @@
#include <lib/mathlib/math/Limits.hpp>
#include <lib/mathlib/math/WelfordMean.hpp>
#include <lib/mathlib/math/WelfordMeanVector.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Accelerometer.hpp>
@@ -92,6 +93,9 @@ private:
void SensorCalibrationSaveAccel();
void SensorCalibrationSaveGyro();
// return the square of two floating point numbers
static constexpr float sq(float var) { return var * var; }
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)};
@@ -119,16 +123,17 @@ private:
hrt_abstime _gyro_timestamp_sample_last{0};
hrt_abstime _gyro_timestamp_last{0};
math::WelfordMean<float, 3> _raw_accel_mean{};
math::WelfordMean<float, 3> _raw_gyro_mean{};
math::WelfordMeanVector<float, 3> _raw_accel_mean{};
math::WelfordMeanVector<float, 3> _raw_gyro_mean{};
math::WelfordMean<float, 2> _accel_interval_mean{};
math::WelfordMean<float, 2> _gyro_interval_mean{};
math::WelfordMean<float> _accel_mean_interval_us{};
math::WelfordMean<float> _accel_fifo_mean_interval_us{};
math::WelfordMean<float, 2> _gyro_update_latency_mean{};
math::WelfordMean<float> _gyro_mean_interval_us{};
math::WelfordMean<float> _gyro_fifo_mean_interval_us{};
float _accel_interval_best_variance{(float)INFINITY};
float _gyro_interval_best_variance{(float)INFINITY};
math::WelfordMean<float> _gyro_update_latency_mean_us{};
math::WelfordMean<float> _gyro_publish_latency_mean_us{};
float _accel_interval_us{NAN};
float _gyro_interval_us{NAN};
@@ -166,7 +171,7 @@ private:
bool _data_gap{false};
bool _update_integrator_config{true};
bool _intervals_configured{false};
bool _publish_status{false};
bool _publish_status{true};
const uint8_t _instance;