zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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)
release/1.12
Daniel Agar 4 years ago committed by GitHub
parent
3fcd323f6a
commit
f0d8d53da6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 89 additions and 89 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 11
      src/modules/sensors/vehicle_imu/Integrator.hpp
  2. 167
      src/modules/sensors/vehicle_imu/VehicleIMU.cpp

11
src/modules/sensors/vehicle_imu/Integrator.hpp
Unescape View File

@ -51,6 +51,9 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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

167
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
Unescape View File

@ -158,10 +158,10 @@ void VehicleIMU::Run() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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()) {
uint32_t accel_integral_dt;
uint32_t gyro_integral_dt;
Vector3f delta_angle;
Vector3f delta_velocity;
if (_accel_integrator.reset(delta_velocity, accel_integral_dt)
&& _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
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 / 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 / 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);
// 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();
// 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;
_status.timestamp = hrt_absolute_time();
_vehicle_imu_status_pub.publish(_status);
_publish_status = false;
}
vehicle_imu_s imu;
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)) {
// 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);
if (_accel_calibration.enabled() && _gyro_calibration.enabled()) {
updated = true;
// 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 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};
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))) {
_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 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);
_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() @@ -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) @@ -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);

Write Preview
Loading…
Cancel
Save