diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 7bca8ca7d1..ea28638a56 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -550,9 +550,11 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // index 37 was NOTCH_ +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED // @Group: LOG_ // @Path: ../AP_InertialSensor/BatchSampler.cpp AP_SUBGROUPINFO(batchsampler, "LOG_", 39, AP_InertialSensor, AP_InertialSensor::BatchSampler), +#endif // @Param: ENABLE_MASK // @DisplayName: IMU enable mask @@ -889,8 +891,10 @@ AP_InertialSensor::init(uint16_t loop_rate) _last_sample_usec = 0; _have_sample = false; +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED // initialise IMU batch logging batchsampler.init(); +#endif #if HAL_WITH_DSP AP_GyroFFT* fft = AP::fft(); @@ -1230,7 +1234,9 @@ AP_InertialSensor::detect_backends(void) // ins_periodic: 57500 events, 0 overruns, 208754us elapsed, 3us avg, min 1us max 218us 40.662us rms void AP_InertialSensor::periodic() { +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED batchsampler.periodic(); +#endif } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 353c03f5e9..4c32f3c500 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -330,6 +330,7 @@ public: IMU_SENSOR_TYPE_GYRO = 1, }; +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED class BatchSampler { public: BatchSampler(const AP_InertialSensor &imu) : @@ -415,6 +416,7 @@ public: const AP_InertialSensor &_imu; }; BatchSampler batchsampler{*this}; +#endif #if HAL_EXTERNAL_AHRS_ENABLED // handle external AHRS data diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index e6b5d388eb..4fd5db99c8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -339,12 +339,17 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, } // 5us +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED if (!_imu.batchsampler.doing_post_filter_logging()) { log_gyro_raw(instance, sample_us, gyro); } else { log_gyro_raw(instance, sample_us, _imu._gyro_filtered[instance]); } +#else + // assume pre-filter logging if batchsampler is not enabled + log_gyro_raw(instance, sample_us, gyro); +#endif } /* @@ -431,12 +436,17 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const _imu._new_gyro_data[instance] = true; } +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED if (!_imu.batchsampler.doing_post_filter_logging()) { log_gyro_raw(instance, sample_us, gyro); } else { log_gyro_raw(instance, sample_us, _imu._gyro_filtered[instance]); } +#else + // assume we're doing pre-filter logging: + log_gyro_raw(instance, sample_us, gyro); +#endif } void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) @@ -450,9 +460,11 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa if (should_log_imu_raw()) { Write_GYR(instance, sample_us, gyro); } else { +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED if (!_imu.batchsampler.doing_sensor_rate_logging()) { _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us, gyro); } +#endif } #endif } @@ -558,11 +570,16 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance, } // 5us +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED if (!_imu.batchsampler.doing_post_filter_logging()) { log_accel_raw(instance, sample_us, accel); } else { log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]); } +#else + // assume we're doing pre-filter logging: + log_accel_raw(instance, sample_us, accel); +#endif } /* @@ -630,29 +647,38 @@ void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, con _imu._new_accel_data[instance] = true; } +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED if (!_imu.batchsampler.doing_post_filter_logging()) { log_accel_raw(instance, sample_us, accel); } else { log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]); } +#else + // assume we're doing pre-filter logging + log_accel_raw(instance, sample_us, accel); +#endif } void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel) { +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED if (!_imu.batchsampler.doing_sensor_rate_logging()) { return; } _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, AP_HAL::micros64(), accel); +#endif } void AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro) { +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED if (!_imu.batchsampler.doing_sensor_rate_logging()) { return; } _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, AP_HAL::micros64(), gyro); +#endif } void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel) @@ -666,9 +692,11 @@ void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t s if (should_log_imu_raw()) { Write_ACC(instance, sample_us, accel); } else { +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED if (!_imu.batchsampler.doing_sensor_rate_logging()) { _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, sample_us, accel); } +#endif } #endif } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index 2e2356cb3f..48f7c41050 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -95,6 +95,7 @@ void AP_InertialSensor::Write_Vibration() const } } +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED // Write information about a series of IMU readings to log: bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) const { @@ -133,6 +134,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt)); } +#endif // @LoggerMessage: FTN // @Description: Filter Tuning Message - per motor diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_config.h index 3d73092e8c..004c14a32e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_config.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_config.h @@ -1,7 +1,12 @@ #pragma once #include +#include #ifndef AP_INERTIALSENSOR_ENABLED #define AP_INERTIALSENSOR_ENABLED 1 #endif + +#ifndef AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED +#define AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_LOGGING_ENABLED) +#endif diff --git a/libraries/AP_InertialSensor/BatchSampler.cpp b/libraries/AP_InertialSensor/BatchSampler.cpp index a0d8e786b5..a125f1a286 100644 --- a/libraries/AP_InertialSensor/BatchSampler.cpp +++ b/libraries/AP_InertialSensor/BatchSampler.cpp @@ -1,6 +1,6 @@ #include "AP_InertialSensor.h" -#if AP_INERTIALSENSOR_ENABLED +#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED #include #include @@ -282,4 +282,4 @@ void AP_InertialSensor::BatchSampler::sample(uint8_t _instance, AP_InertialSenso data_write_offset++; // may unblock the reading process #endif } -#endif //#if AP_INERTIALSENSOR_ENABLED +#endif //#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED