mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 06:44:08 +08:00
sensors/vehicle_imu: minor IMU integration improvements
- IMU integrator set max dt based on final return size (uint16) - improve integration consuming gyro as needed then integrate all available accel until caught up - increase required number of samples for sensor rate measurement (online Welford mean)
This commit is contained in:
Daniel Agar
GitHub
parent
3fcd323f6a
commit
f0d8d53da6
@ -51,6 +51,9 @@ public:
|
||||
Integrator() = default;
|
||||
~Integrator() = default;
|
||||
|
||||
static constexpr float DT_MIN{FLT_MIN};
|
||||
static constexpr float DT_MAX{static_cast<float>(UINT16_MAX) * 1e-6f};
|
||||
|
||||
/**
|
||||
* Put an item into the integral.
|
||||
*
|
||||
@ -60,7 +63,7 @@ public:
|
||||
*/
|
||||
inline void put(const matrix::Vector3f &val, const float dt)
|
||||
{
|
||||
if (dt > 0.f && dt <= (_reset_interval_min * 2.f)) {
|
||||
if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) {
|
||||
_alpha += integrate(val, dt);
|
||||
|
||||
} else {
|
||||
@ -103,7 +106,7 @@ public:
|
||||
* @param integral_dt Get the dt in us of the current integration.
|
||||
* @return true if integral valid
|
||||
*/
|
||||
bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
|
||||
bool reset(matrix::Vector3f &integral, uint16_t &integral_dt)
|
||||
{
|
||||
if (integral_ready()) {
|
||||
integral = _alpha;
|
||||
@ -155,7 +158,7 @@ public:
|
||||
*/
|
||||
inline void put(const matrix::Vector3f &val, const float dt)
|
||||
{
|
||||
if (dt > 0.f && dt <= (_reset_interval_min * 2.f)) {
|
||||
if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) {
|
||||
// Use trapezoidal integration to calculate the delta integral
|
||||
const matrix::Vector3f delta_alpha{integrate(val, dt)};
|
||||
|
||||
@ -190,7 +193,7 @@ public:
|
||||
* @param integral_dt Get the dt in us of the current integration.
|
||||
* @return true if integral valid
|
||||
*/
|
||||
bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
|
||||
bool reset(matrix::Vector3f &integral, uint16_t &integral_dt)
|
||||
{
|
||||
if (Integrator::reset(integral, integral_dt)) {
|
||||
// apply coning corrections
|
||||
|
||||
@ -158,10 +158,10 @@ 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.valid()
|
||||
&& (_gyro_update_latency_mean.mean()(1) > (1.5f * _gyro_interval_us * 1e-6f))) {
|
||||
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 us",
|
||||
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));
|
||||
|
||||
@ -176,11 +176,11 @@ void VehicleIMU::Run()
|
||||
}
|
||||
|
||||
|
||||
bool consume_all_accel = !_intervals_configured || _data_gap
|
||||
|| (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us));
|
||||
|
||||
// update accel until integrator ready and caught up to gyro
|
||||
if (!_accel_integrator.integral_ready() || consume_all_accel) {
|
||||
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)))) {
|
||||
|
||||
if (UpdateAccel()) {
|
||||
updated = true;
|
||||
}
|
||||
@ -192,11 +192,12 @@ void VehicleIMU::Run()
|
||||
}
|
||||
|
||||
// check for additional updates and that we're fully caught up before publishing
|
||||
if ((consume_all_gyro && _sensor_gyro_sub.updated()) || (consume_all_accel && _sensor_accel_sub.updated())) {
|
||||
if (consume_all_gyro && _sensor_gyro_sub.updated()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (_intervals_configured) {
|
||||
// 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) {
|
||||
@ -205,9 +206,8 @@ void VehicleIMU::Run()
|
||||
}
|
||||
|
||||
const float time_run_s = now_us * 1e-6f;
|
||||
const float time_gyro_timestamp_last_s = _gyro_timestamp_last * 1e-6f;
|
||||
const float time_gyro_timestamp_sample_last_s = _gyro_timestamp_sample_last * 1e-6f;
|
||||
_gyro_update_latency_mean.update(Vector2f{time_run_s - time_gyro_timestamp_sample_last_s, time_run_s - time_gyro_timestamp_last_s});
|
||||
|
||||
_gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f});
|
||||
|
||||
return;
|
||||
}
|
||||
@ -242,7 +242,7 @@ bool VehicleIMU::UpdateAccel()
|
||||
_accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples});
|
||||
}
|
||||
|
||||
if (_accel_interval_mean.valid()
|
||||
if (_accel_interval_mean.valid() && (_accel_interval_mean.count() > 100 || !PX4_ISFINITE(_accel_interval_best_variance))
|
||||
&& ((_accel_interval_mean.variance()(0) < _accel_interval_best_variance) || (_accel_interval_mean.count() > 1000))) {
|
||||
// update sample rate if previously invalid or changed
|
||||
const float interval_delta_us = fabsf(_accel_interval_mean.mean()(0) - _accel_interval_us);
|
||||
@ -340,7 +340,7 @@ bool VehicleIMU::UpdateGyro()
|
||||
// integrate queued gyro
|
||||
sensor_gyro_s gyro;
|
||||
|
||||
while (_sensor_gyro_sub.update(&gyro)) {
|
||||
if (_sensor_gyro_sub.update(&gyro)) {
|
||||
if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) {
|
||||
_data_gap = true;
|
||||
perf_count(_gyro_generation_gap_perf);
|
||||
@ -355,7 +355,7 @@ bool VehicleIMU::UpdateGyro()
|
||||
_gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples});
|
||||
}
|
||||
|
||||
if (_gyro_interval_mean.valid()
|
||||
if (_gyro_interval_mean.valid() && (_gyro_interval_mean.count() > 100 || !PX4_ISFINITE(_gyro_interval_best_variance))
|
||||
&& ((_gyro_interval_mean.variance()(0) < _gyro_interval_best_variance) || (_gyro_interval_mean.count() > 1000))) {
|
||||
// update sample rate if previously invalid or changed
|
||||
const float interval_delta_us = fabsf(_gyro_interval_mean.mean()(0) - _gyro_interval_us);
|
||||
@ -411,80 +411,73 @@ bool VehicleIMU::Publish()
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
// publish if both accel & gyro integrators are ready
|
||||
if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) {
|
||||
vehicle_imu_s imu;
|
||||
Vector3f delta_angle;
|
||||
Vector3f delta_velocity;
|
||||
|
||||
uint32_t accel_integral_dt;
|
||||
uint32_t gyro_integral_dt;
|
||||
Vector3f delta_angle;
|
||||
Vector3f delta_velocity;
|
||||
if (_accel_integrator.reset(delta_velocity, imu.delta_velocity_dt)
|
||||
&& _gyro_integrator.reset(delta_angle, imu.delta_angle_dt)) {
|
||||
|
||||
if (_accel_integrator.reset(delta_velocity, accel_integral_dt)
|
||||
&& _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
|
||||
if (_accel_calibration.enabled() && _gyro_calibration.enabled()) {
|
||||
|
||||
if (_accel_calibration.enabled() && _gyro_calibration.enabled()) {
|
||||
// delta angle: apply offsets, scale, and board rotation
|
||||
_gyro_calibration.SensorCorrectionsUpdate();
|
||||
const float gyro_dt_inv = 1.e6f / imu.delta_angle_dt;
|
||||
const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv};
|
||||
|
||||
// delta angle: apply offsets, scale, and board rotation
|
||||
_gyro_calibration.SensorCorrectionsUpdate();
|
||||
const float gyro_dt_inv = 1.e6f / gyro_integral_dt;
|
||||
const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv};
|
||||
// delta velocity: apply offsets, scale, and board rotation
|
||||
_accel_calibration.SensorCorrectionsUpdate();
|
||||
const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt;
|
||||
Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv};
|
||||
|
||||
// delta velocity: apply offsets, scale, and board rotation
|
||||
_accel_calibration.SensorCorrectionsUpdate();
|
||||
const float accel_dt_inv = 1.e6f / accel_integral_dt;
|
||||
Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv};
|
||||
UpdateAccelVibrationMetrics(delta_velocity_corrected);
|
||||
UpdateGyroVibrationMetrics(delta_angle_corrected);
|
||||
|
||||
UpdateAccelVibrationMetrics(delta_velocity_corrected);
|
||||
UpdateGyroVibrationMetrics(delta_angle_corrected);
|
||||
// vehicle_imu_status
|
||||
// publish before vehicle_imu so that error counts are available synchronously if needed
|
||||
if ((_accel_sum_count > 0) && (_gyro_sum_count > 0)
|
||||
&& (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) {
|
||||
|
||||
// vehicle_imu_status
|
||||
// publish before vehicle_imu so that error counts are available synchronously if needed
|
||||
if (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
|
||||
_status.accel_device_id = _accel_calibration.device_id();
|
||||
_status.gyro_device_id = _gyro_calibration.device_id();
|
||||
_status.accel_device_id = _accel_calibration.device_id();
|
||||
_status.gyro_device_id = _gyro_calibration.device_id();
|
||||
|
||||
// mean accel
|
||||
const Vector3f accel_mean{_accel_calibration.Correct(_accel_sum / _accel_sum_count)};
|
||||
accel_mean.copyTo(_status.mean_accel);
|
||||
_status.temperature_accel = _accel_temperature / _accel_sum_count;
|
||||
_accel_sum.zero();
|
||||
_accel_temperature = 0;
|
||||
_accel_sum_count = 0;
|
||||
// mean accel
|
||||
const Vector3f accel_mean{_accel_calibration.Correct(_accel_sum / _accel_sum_count)};
|
||||
accel_mean.copyTo(_status.mean_accel);
|
||||
_status.temperature_accel = _accel_temperature / _accel_sum_count;
|
||||
_accel_sum.zero();
|
||||
_accel_temperature = 0;
|
||||
_accel_sum_count = 0;
|
||||
|
||||
// mean gyro
|
||||
const Vector3f gyro_mean{_gyro_calibration.Correct(_gyro_sum / _gyro_sum_count)};
|
||||
gyro_mean.copyTo(_status.mean_gyro);
|
||||
_status.temperature_gyro = _gyro_temperature / _gyro_sum_count;
|
||||
_gyro_sum.zero();
|
||||
_gyro_temperature = 0;
|
||||
_gyro_sum_count = 0;
|
||||
// mean gyro
|
||||
const Vector3f gyro_mean{_gyro_calibration.Correct(_gyro_sum / _gyro_sum_count)};
|
||||
gyro_mean.copyTo(_status.mean_gyro);
|
||||
_status.temperature_gyro = _gyro_temperature / _gyro_sum_count;
|
||||
_gyro_sum.zero();
|
||||
_gyro_temperature = 0;
|
||||
_gyro_sum_count = 0;
|
||||
|
||||
_status.timestamp = hrt_absolute_time();
|
||||
_vehicle_imu_status_pub.publish(_status);
|
||||
_status.timestamp = hrt_absolute_time();
|
||||
_vehicle_imu_status_pub.publish(_status);
|
||||
|
||||
_publish_status = false;
|
||||
}
|
||||
|
||||
|
||||
// publish vehicle_imu
|
||||
vehicle_imu_s imu;
|
||||
imu.timestamp_sample = _gyro_timestamp_sample_last;
|
||||
imu.accel_device_id = _accel_calibration.device_id();
|
||||
imu.gyro_device_id = _gyro_calibration.device_id();
|
||||
delta_angle_corrected.copyTo(imu.delta_angle);
|
||||
delta_velocity_corrected.copyTo(imu.delta_velocity);
|
||||
imu.delta_angle_dt = gyro_integral_dt;
|
||||
imu.delta_velocity_dt = accel_integral_dt;
|
||||
imu.delta_velocity_clipping = _delta_velocity_clipping;
|
||||
imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count();
|
||||
imu.timestamp = hrt_absolute_time();
|
||||
_vehicle_imu_pub.publish(imu);
|
||||
|
||||
updated = true;
|
||||
_publish_status = false;
|
||||
}
|
||||
|
||||
// publish vehicle_imu
|
||||
imu.timestamp_sample = _gyro_timestamp_sample_last;
|
||||
imu.accel_device_id = _accel_calibration.device_id();
|
||||
imu.gyro_device_id = _gyro_calibration.device_id();
|
||||
delta_angle_corrected.copyTo(imu.delta_angle);
|
||||
delta_velocity_corrected.copyTo(imu.delta_velocity);
|
||||
imu.delta_velocity_clipping = _delta_velocity_clipping;
|
||||
imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count();
|
||||
imu.timestamp = hrt_absolute_time();
|
||||
_vehicle_imu_pub.publish(imu);
|
||||
|
||||
// reset clip counts
|
||||
_delta_velocity_clipping = 0;
|
||||
|
||||
updated = true;
|
||||
}
|
||||
}
|
||||
|
||||
@ -513,11 +506,12 @@ void VehicleIMU::UpdateIntegratorConfiguration()
|
||||
_accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval_us));
|
||||
_accel_integrator.set_reset_samples(accel_integral_samples);
|
||||
|
||||
_backup_schedule_timeout_us = sensor_accel_s::ORB_QUEUE_LENGTH * _accel_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::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us,
|
||||
sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us);
|
||||
|
||||
// gyro: find largest integer multiple of gyro_integral_samples
|
||||
for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) {
|
||||
if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) {
|
||||
@ -565,12 +559,15 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
|
||||
|
||||
void VehicleIMU::PrintStatus()
|
||||
{
|
||||
|
||||
PX4_INFO("%" PRIu8 " - Accel ID: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro ID: %" PRIu32
|
||||
", interval: %.1f us (SD %.1f us)",
|
||||
_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));
|
||||
|
||||
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));
|
||||
|
||||
perf_print_counter(_accel_generation_gap_perf);
|
||||
perf_print_counter(_gyro_generation_gap_perf);
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user