diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index cad3d951ef..917ecc0d4e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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 isbh_sent : 1; - bool _doing_sensor_rate_logging : 1; - bool _doing_post_filter_logging : 1; - uint8_t instance : 3; // instance we are sending data for - AP_InertialSensor::IMU_SENSOR_TYPE type : 1; + bool initialised; + bool isbh_sent; + bool _doing_sensor_rate_logging; + bool _doing_post_filter_logging; + bool _doing_pre_post_filter_logging; + 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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index b4e0e5385d..27b9a86cef 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/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, diff --git a/libraries/AP_InertialSensor/BatchSampler.cpp b/libraries/AP_InertialSensor/BatchSampler.cpp index 77d741bc7b..cd35ff42cf 100644 --- a/libraries/AP_InertialSensor/BatchSampler.cpp +++ b/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. - // @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP), 1: Sample post-filtering + // @Description: Options for the BatchSampler. + // @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 ((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_POST_FILTER) { + if (has_option(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)) { + if (has_option(BATCH_OPT_PRE_POST_FILTER)) { + _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< (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<