diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 7c80d92066024a..80d125665a0c5e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -674,6 +674,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { AP_SUBGROUPINFO(params[1], "5_", 55, AP_InertialSensor, AP_InertialSensor_Params), #endif + // @Param: _RAW_LOG_OPT + // @DisplayName: Raw logging options + // @Description: Raw logging options bitmask + // @Bitmask: 0:Log primary gyro only, 1:Log all gyros, 2:Post filter, 3: Pre and post filter + // @User: Advanced + AP_GROUPINFO("_RAW_LOG_OPT", 56, AP_InertialSensor, raw_logging_options, 0), + /* NOTE: parameter indexes have gaps above. When adding new parameters check for conflicts carefully diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 1f00247aba8b2f..27d21e1137d164 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -766,6 +766,18 @@ class AP_InertialSensor : AP_AccelCal_Client AP_Int32 tcal_options; bool tcal_learning; #endif + + // Raw logging options bitmask and parameter + enum class RAW_LOGGING_OPTION { + PRIMARY_GYRO_ONLY = (1U<<0), + ALL_GYROS = (1U<<1), + POST_FILTER = (1U<<2), + PRE_AND_POST_FILTER = (1U<<3), + }; + AP_Int16 raw_logging_options; + bool raw_logging_option_set(RAW_LOGGING_OPTION option) const { + return (raw_logging_options.get() & int32_t(option)) != 0; + } }; namespace AP { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 88b780964e7911..9b2954ef86a493 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -343,17 +343,7 @@ 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 + log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]); } /* @@ -440,20 +430,10 @@ 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 + log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]); } -void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) +void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &raw_gyro, const Vector3f &filtered_gyro) { #if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); @@ -461,12 +441,30 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa // should not have been called return; } - if (should_log_imu_raw()) { - Write_GYR(instance, sample_us, gyro); + + if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::ALL_GYROS) || + (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRIMARY_GYRO_ONLY) && (instance == AP::ahrs().get_primary_gyro_index())) || + should_log_imu_raw()) { + + if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRE_AND_POST_FILTER)) { + // Both pre and post, offset post instance as batch sampler does + Write_GYR(instance, sample_us, raw_gyro); + Write_GYR(instance + _imu._gyro_count, sample_us, filtered_gyro); + + } else if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::POST_FILTER)) { + // Just post + Write_GYR(instance, sample_us, filtered_gyro); + + } else { + // Just pre + Write_GYR(instance, sample_us, raw_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); + _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us, + !_imu.batchsampler.doing_post_filter_logging() ? raw_gyro : filtered_gyro); } #endif } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index c49356a85ffc9a..a9bd7bb4118081 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -315,7 +315,7 @@ class AP_InertialSensor_Backend bool should_log_imu_raw() const ; void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel) __RAMFUNC__; - void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gryo) __RAMFUNC__; + void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &raw_gyro, const Vector3f &filtered_gyro) __RAMFUNC__; // logging void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const __RAMFUNC__; // Write ACC data packet: raw accel data