AP_InertialSensor: allow concurrent logging of both pre- and post-filter IMU data for FFT

remove batch logging bitfields

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -342,7 +342,10 @@ public:
         void periodic();
 
         bool doing_sensor_rate_logging() const { return _doing_sensor_rate_logging; }
-        bool doing_post_filter_logging() const { return _doing_post_filter_logging; }
+        bool doing_post_filter_logging() const {
+            return (_doing_post_filter_logging && (post_filter || !_doing_sensor_rate_logging))
+                || (_doing_pre_post_filter_logging && post_filter);
+        }
 
         // class level parameters
         static const struct AP_Param::GroupInfo var_info[];
@@ -370,6 +373,7 @@ public:
         enum batch_opt_t {
             BATCH_OPT_SENSOR_RATE = (1<<0),
             BATCH_OPT_POST_FILTER = (1<<1),
+            BATCH_OPT_PRE_POST_FILTER = (1<<2),
         };
 
         void rotate_to_next_sensor();
@@ -382,14 +386,18 @@ public:
         bool Write_ISBH(const float sample_rate_hz) const;
         bool Write_ISBD() const;
 
+        bool has_option(batch_opt_t option) const { return _batch_options_mask & uint16_t(option); }
+
         uint64_t measurement_started_us;
 
-        bool initialised : 1;
+        bool initialised;
-        bool isbh_sent : 1;
+        bool isbh_sent;
-        bool _doing_sensor_rate_logging : 1;
+        bool _doing_sensor_rate_logging;
-        bool _doing_post_filter_logging : 1;
+        bool _doing_post_filter_logging;
-        uint8_t instance : 3; // instance we are sending data for
+        bool _doing_pre_post_filter_logging;
-        AP_InertialSensor::IMU_SENSOR_TYPE type : 1;
+        uint8_t instance; // instance we are sending data for
+        bool post_filter; // whether we are sending post-filter data
+        AP_InertialSensor::IMU_SENSOR_TYPE type;
         uint16_t isb_seqnum;
         int16_t *data_x;
         int16_t *data_y;

libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

@@ -98,12 +98,17 @@ void AP_InertialSensor::Write_Vibration() const
 // Write information about a series of IMU readings to log:
 bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) const
 {
+    uint8_t instance_to_write = instance;
+    if (post_filter && (_doing_pre_post_filter_logging
+            || (_doing_post_filter_logging && _doing_sensor_rate_logging))) {
+        instance_to_write += (type == IMU_SENSOR_TYPE_ACCEL ? _imu._accel_count : _imu._gyro_count);
+    }
     const struct log_ISBH pkt{
         LOG_PACKET_HEADER_INIT(LOG_ISBH_MSG),
         time_us        : AP_HAL::micros64(),
         seqno          : isb_seqnum,
         sensor_type    : (uint8_t)type,
-        instance       : instance,
+        instance       : instance_to_write,
         multiplier     : multiplier,
         sample_count   : (uint16_t)_required_count,
         sample_us      : measurement_started_us,

libraries/AP_InertialSensor/BatchSampler.cpp

@@ -24,8 +24,8 @@ const AP_Param::GroupInfo AP_InertialSensor::BatchSampler::var_info[] = {
 
     // @Param: BAT_OPT
     // @DisplayName: Batch Logging Options Mask
-    // @Description: Options for the BatchSampler. Post-filter and sensor-rate logging cannot be used at the same time.
+    // @Description: Options for the BatchSampler.
-    // @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP), 1: Sample post-filtering
+    // @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP), 1: Sample post-filtering, 2: Sample pre- and post-filter
     // @User: Advanced
     AP_GROUPINFO("BAT_OPT",  3, AP_InertialSensor::BatchSampler, _batch_options_mask, 0),
 
@@ -90,14 +90,13 @@ void AP_InertialSensor::BatchSampler::periodic()
 
 void AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging()
 {
-    // We can't do post-filter sensor rate logging
+    if (has_option(BATCH_OPT_POST_FILTER)) {
-    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 (has_option(BATCH_OPT_PRE_POST_FILTER)) {
-    if (!((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_SENSOR_RATE)) {
+        _doing_pre_post_filter_logging = true;
+    }
+    if (!has_option(BATCH_OPT_SENSOR_RATE)) {
         _doing_sensor_rate_logging = false;
         return;
     }
@@ -121,6 +120,7 @@ void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
     if ((1U<<instance) > (uint8_t)_sensor_mask) {
         // should only ever happen if user resets _sensor_mask
         instance = 0;
+        post_filter = false;
     }
 
     if (type == IMU_SENSOR_TYPE_ACCEL) {
@@ -154,6 +154,7 @@ void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
             if (_sensor_mask & (1U<<i)) {
                 instance = i;
                 haveinstance = true;
+                post_filter = !post_filter;
                 break;
             }
         }
@@ -164,6 +165,7 @@ void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
         abort();
 #endif
         instance = 0;
+        post_filter = false;
         return;
     }