fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: VehicleIMU improve scheduling

Browse Source 
- schedule on interval multiple of sensor_gyro publications without increasing the configured integration rate
 - IMU_INTEG_RATE add enums to guide appropriate selection
 - add average calculation bounds to prevent incorrect calculation and force monitoring again if there are consecutive gaps in data
This commit is contained in:
Daniel Agar 2020-07-09 10:29:04 -04:00 committed by GitHub
parent fa8ba09f50
commit f3d923f226
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 85 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -108,19 +108,19 @@ void VehicleIMU::ParametersUpdate(bool force)
_accel_calibration.ParametersUpdate();
_gyro_calibration.ParametersUpdate();
// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), 50, 1000);
// constrain IMU integration time 1-10 milliseconds (100-1000 Hz)
int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(),
100, math::max(_param_imu_gyro_ratemax.get(), 1000));
if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
PX4_WARN("IMU_INTEG_RATE updated %d -> %d", _param_imu_integ_rate.get(), imu_integration_rate_hz);
_param_imu_integ_rate.set(imu_integration_rate_hz);
_param_imu_integ_rate.commit_no_notification();
}
if (_param_imu_integ_rate.get() != imu_integ_rate_prev) {
// force update
_intervals_update = true;
_accel_interval.timestamp_sample_last = 0;
_gyro_interval.timestamp_sample_last = 0;
UpdateIntegratorConfiguration();
}
}
}
@ -129,8 +129,21 @@ bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstim
{
bool updated = false;
if ((intavg.timestamp_sample_last > 0) && (timestamp_sample > intavg.timestamp_sample_last)) {
intavg.interval_sum += (timestamp_sample - intavg.timestamp_sample_last);
// conservative maximum time between samples to reject large gaps and reset averaging
float max_interval_us = 10000; // 100 Hz
float min_interval_us = 100; // 10,000 Hz
if (intavg.update_interval > 0) {
// if available use previously calculated interval as bounds
max_interval_us = 1.5f * intavg.update_interval;
min_interval_us = 0.5f * intavg.update_interval;
}
const float interval_us = (timestamp_sample - intavg.timestamp_sample_last);
if ((intavg.timestamp_sample_last > 0) && (interval_us < max_interval_us) && (interval_us > min_interval_us)) {
intavg.interval_sum += interval_us;
intavg.interval_count++;
// periodically calculate sensor update rate
@ -170,6 +183,7 @@ void VehicleIMU::Run()
ParametersUpdate();
bool sensor_data_gap = false;
bool update_integrator_config = false;
bool publish_status = false;
@ -180,11 +194,18 @@ void VehicleIMU::Run()
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);
// if there's a gap in data start monitoring publication interval again
_intervals_update = true;
_gyro_interval.timestamp_sample_last = 0;
_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)) {
update_integrator_config = true;
publish_status = true;
_status.gyro_rate_hz = roundf(1e6f / _gyro_interval.update_interval);
}
}
_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
@ -199,14 +220,7 @@ void VehicleIMU::Run()
_gyro_integrator.put(gyro.timestamp_sample, Vector3f{gyro.x, gyro.y, gyro.z});
_last_timestamp_sample_gyro = gyro.timestamp_sample;
// collect sample interval average for filters
if (_intervals_update && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
update_integrator_config = true;
publish_status = true;
_status.gyro_rate_hz = roundf(1e6f / _gyro_interval.update_interval);
}
if (_intervals_configured && _gyro_integrator.integral_ready()) {
if (!sensor_data_gap && _intervals_configured && _gyro_integrator.integral_ready()) {
break;
}
}
@ -218,11 +232,18 @@ 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;
perf_count(_accel_generation_gap_perf);
// if there's a gap in data start monitoring publication interval again
_intervals_update = true;
_accel_interval.timestamp_sample_last = 0;
_accel_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging
} else {
// collect sample interval average for filters
if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
update_integrator_config = true;
publish_status = true;
_status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
}
}
_accel_last_generation = _sensor_accel_sub.get_last_generation();
@ -237,13 +258,6 @@ void VehicleIMU::Run()
_accel_integrator.put(accel.timestamp_sample, Vector3f{accel.x, accel.y, accel.z});
_last_timestamp_sample_accel = accel.timestamp_sample;
// collect sample interval average for filters
if (_intervals_update && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
update_integrator_config = true;
publish_status = true;
_status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
}
if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
// rotate sensor clip counts into vehicle body frame
@ -275,13 +289,25 @@ void VehicleIMU::Run()
}
// break once caught up to gyro
if (_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++;
// if there's consistently a gap in data start monitoring publication interval again
if (_consecutive_data_gap > 10) {
_intervals_configured = false;
}
} else {
_consecutive_data_gap = 0;
}
// reconfigure integrators if calculated sensor intervals have changed
if (update_integrator_config) {
UpdateIntegratorConfiguration();
@ -349,11 +375,8 @@ void VehicleIMU::UpdateIntegratorConfiguration()
const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
// determine number of sensor samples that will get closest to the desired integration interval
const uint8_t accel_integral_samples = constrain(roundf(configured_interval_us / _accel_interval.update_interval),
1.f, (float)sensor_accel_s::ORB_QUEUE_LENGTH);
const uint8_t gyro_integral_samples = constrain(roundf(configured_interval_us / _gyro_interval.update_interval),
1.f, (float)sensor_gyro_s::ORB_QUEUE_LENGTH);
const uint8_t accel_integral_samples = math::max(1.f, roundf(configured_interval_us / _accel_interval.update_interval));
const uint8_t gyro_integral_samples = math::max(1.f, roundf(configured_interval_us / _gyro_interval.update_interval));
// let the gyro set the configuration and scheduling
// accel integrator will be forced to reset when gyro integrator is ready
@ -364,18 +387,23 @@ void VehicleIMU::UpdateIntegratorConfiguration()
_accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval.update_interval));
_gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval.update_interval));
_sensor_accel_sub.set_required_updates(accel_integral_samples);
_sensor_gyro_sub.set_required_updates(gyro_integral_samples);
// 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 % n == 0) {
_sensor_gyro_sub.set_required_updates(n);
// run when there are enough new gyro samples, unregister accel
_sensor_accel_sub.unregisterCallback();
// run when there are enough new gyro samples, unregister accel
_sensor_accel_sub.unregisterCallback();
_intervals_configured = true;
_intervals_update = false; // stop monitoring topic publication rate
_intervals_configured = true; // stop monitoring topic publication rates
PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f, gyro interval: %.1f",
_accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples,
(double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, 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.update_interval, (double)_gyro_interval.update_interval, n);
break;
}
}
}
}
@ -403,9 +431,14 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
void VehicleIMU::PrintStatus()
{
PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us",
_accel_calibration.device_id(), (double)_accel_interval.update_interval,
_gyro_calibration.device_id(), (double)_gyro_interval.update_interval);
if (_accel_calibration.device_id() == _gyro_calibration.device_id()) {
PX4_INFO("IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _accel_calibration.device_id(),
(double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
} else {
PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _accel_calibration.device_id(),
(double)_accel_interval.update_interval, _gyro_calibration.device_id(), (double)_gyro_interval.update_interval);
}
perf_print_counter(_accel_generation_gap_perf);
perf_print_counter(_gyro_generation_gap_perf);

5
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -105,6 +105,7 @@ private:
unsigned _accel_last_generation{0};
unsigned _gyro_last_generation{0};
unsigned _consecutive_data_gap{0};
matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement
matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement
@ -113,7 +114,6 @@ private:
uint8_t _delta_velocity_clipping{0};
bool _intervals_update{true};
bool _intervals_configured{false};
perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")};
@ -122,7 +122,8 @@ private:
perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax
)
};

3
src/modules/sensors/vehicle_imu/imu_parameters.c
View File

@ -39,6 +39,9 @@
*
* @min 100
* @max 1000
* @value 100 100 Hz
* @value 200 200 Hz
* @value 400 400 Hz
* @unit Hz
* @reboot_required true
* @group Sensors