Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

sensors/vehicle_imu: move accel & gyro updates to separate methods

Browse Source
This commit is contained in:
Daniel Agar
2021-03-08 18:21:32 -05:00
parent a5979e16be
commit 3340fca332
4 changed files with 81 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_imu/CMakeLists.txt
+1 -1
View File
@@ -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
src/modules/sensors/vehicle_imu/Integrator.hpp
+1 -1
View File
@@ -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
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+70 -62
View File
@@ -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()
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+9 -6
View File
@@ -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 &timestamp_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_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::PublicationMulti<vehicle_imu_status_s> _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};