diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index 771d2d0851..8176327f4d 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/modules/sensors/vehicle_imu/Integrator.hpp index ede1559158..38b0e8fcd1 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.hpp +++ b/src/modules/sensors/vehicle_imu/Integrator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 993de36e06..3a98b6b3b1 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -208,59 +208,34 @@ void VehicleIMU::Run() return; } + UpdateGyro(); + UpdateAccel(); - bool sensor_data_gap = false; - bool update_integrator_config = false; + if (_sensor_data_gap) { + _consecutive_data_gap++; - // integrate queued gyro - sensor_gyro_s gyro; - - while (_sensor_gyro_sub.update(&gyro)) { - perf_count_interval(_gyro_update_perf, gyro.timestamp_sample); - - if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { - sensor_data_gap = true; - perf_count(_gyro_generation_gap_perf); - - _gyro_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging - - } else { - // collect sample interval average for filters - if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample, gyro.samples)) { - update_integrator_config = true; - _publish_status = true; - _status.gyro_rate_hz = 1e6f / _gyro_interval.update_interval; - _status.gyro_raw_rate_hz = 1e6f / _gyro_interval.update_interval_raw; - } + // if there's consistently a gap in data start monitoring publication interval again + if (_consecutive_data_gap > 10) { + _intervals_configured = false; } - _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); + _sensor_data_gap = false; - _gyro_calibration.set_device_id(gyro.device_id); - - if (gyro.error_count != _status.gyro_error_count) { - _publish_status = true; - _status.gyro_error_count = gyro.error_count; - } - - const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z}; - _gyro_sum += gyro_raw; - _gyro_temperature += gyro.temperature; - _gyro_sum_count++; - - const float dt = (gyro.timestamp_sample - _last_timestamp_sample_gyro) * 1e-6f; - _last_timestamp_sample_gyro = gyro.timestamp_sample; - - _gyro_integrator.put(gyro_raw, dt); - - // break if interval is configured and we haven't fallen behind - if (_intervals_configured && _gyro_integrator.integral_ready() - && (hrt_elapsed_time(&gyro.timestamp) < _imu_integration_interval_us) && !sensor_data_gap) { - - break; - } + } else { + _consecutive_data_gap = 0; } + // reconfigure integrators if calculated sensor intervals have changed + if (_update_integrator_config) { + UpdateIntegratorConfiguration(); + _update_integrator_config = false; + } + + Publish(); +} + +void VehicleIMU::UpdateAccel() +{ // update accel, stopping once caught up to the last gyro sample sensor_accel_s accel; @@ -268,7 +243,7 @@ void VehicleIMU::Run() perf_count_interval(_accel_update_perf, accel.timestamp_sample); if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) { - sensor_data_gap = true; + _sensor_data_gap = true; perf_count(_accel_generation_gap_perf); _accel_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging @@ -276,7 +251,7 @@ void VehicleIMU::Run() } else { // collect sample interval average for filters if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample, accel.samples)) { - update_integrator_config = true; + _update_integrator_config = true; _publish_status = true; _status.accel_rate_hz = 1e6f / _accel_interval.update_interval; _status.accel_raw_rate_hz = 1e6f / _accel_interval.update_interval_raw; @@ -344,31 +319,64 @@ void VehicleIMU::Run() } // break once caught up to gyro - if (!sensor_data_gap && _intervals_configured + if (!_sensor_data_gap && _intervals_configured && (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval))) { break; } } +} - if (sensor_data_gap) { - _consecutive_data_gap++; +void VehicleIMU::UpdateGyro() +{ + // integrate queued gyro + sensor_gyro_s gyro; - // if there's consistently a gap in data start monitoring publication interval again - if (_consecutive_data_gap > 10) { - _intervals_configured = false; + while (_sensor_gyro_sub.update(&gyro)) { + perf_count_interval(_gyro_update_perf, gyro.timestamp_sample); + + if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { + _sensor_data_gap = true; + perf_count(_gyro_generation_gap_perf); + + _gyro_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging + + } else { + // collect sample interval average for filters + if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample, gyro.samples)) { + _update_integrator_config = true; + _publish_status = true; + _status.gyro_rate_hz = 1e6f / _gyro_interval.update_interval; + _status.gyro_raw_rate_hz = 1e6f / _gyro_interval.update_interval_raw; + } } - } else { - _consecutive_data_gap = 0; - } + _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); - // reconfigure integrators if calculated sensor intervals have changed - if (update_integrator_config) { - UpdateIntegratorConfiguration(); - } + _gyro_calibration.set_device_id(gyro.device_id); - Publish(); + if (gyro.error_count != _status.gyro_error_count) { + _publish_status = true; + _status.gyro_error_count = gyro.error_count; + } + + const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z}; + _gyro_sum += gyro_raw; + _gyro_temperature += gyro.temperature; + _gyro_sum_count++; + + const float dt = (gyro.timestamp_sample - _last_timestamp_sample_gyro) * 1e-6f; + _last_timestamp_sample_gyro = gyro.timestamp_sample; + + _gyro_integrator.put(gyro_raw, dt); + + // break if interval is configured and we haven't fallen behind + if (_intervals_configured && _gyro_integrator.integral_ready() + && (hrt_elapsed_time(&gyro.timestamp) < _imu_integration_interval_us) && !_sensor_data_gap) { + + break; + } + } } void VehicleIMU::Publish() diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index a8c2a7753c..f65d4babd5 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -72,10 +72,6 @@ public: void PrintStatus(); private: - void ParametersUpdate(bool force = false); - void Publish(); - void Run() override; - struct IntervalAverage { hrt_abstime timestamp_sample_last{0}; uint32_t interval_sum{0}; @@ -85,10 +81,15 @@ private: float update_interval_raw{0.f}; }; + void ParametersUpdate(bool force = false); + void Publish(); + void Run() override; + void UpdateAccel(); + void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); + void UpdateGyro(); + void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample, uint8_t samples = 1); void UpdateIntegratorConfiguration(); - void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); - void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; @@ -134,6 +135,8 @@ private: uint64_t _last_clipping_notify_total_count{0}; orb_advert_t _mavlink_log_pub{nullptr}; + bool _update_integrator_config{false}; + bool _sensor_data_gap{false}; bool _intervals_configured{false}; bool _publish_status{false};