From 31ea3466af8ed3922722dc95ad40cc094a82b503 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 25 May 2019 22:01:40 +0100 Subject: [PATCH] AP_InertialSensor: Post-filter logging takes precedence over sensor-rate logging. --- libraries/AP_InertialSensor/BatchSampler.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_InertialSensor/BatchSampler.cpp b/libraries/AP_InertialSensor/BatchSampler.cpp index b570c04df6..0da3f31bb5 100644 --- a/libraries/AP_InertialSensor/BatchSampler.cpp +++ b/libraries/AP_InertialSensor/BatchSampler.cpp @@ -87,14 +87,15 @@ void AP_InertialSensor::BatchSampler::periodic() void AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging() { - if (((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_POST_FILTER) - && type == IMU_SENSOR_TYPE_GYRO) { + // We can't do post-filter sensor rate logging + if ((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_POST_FILTER) { _doing_post_filter_logging = true; + _doing_sensor_rate_logging = false; return; } + _doing_post_filter_logging = false; if (!((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_SENSOR_RATE)) { _doing_sensor_rate_logging = false; - _doing_post_filter_logging = false; return; } const uint8_t bit = (1<