Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_InertialSensor: use batch logging options to allow pre-post raw gyro logging #24311

Merged
merged 1 commit into from
Oct 2, 2023
Merged

AP_InertialSensor: use batch logging options to allow pre-post raw gyro logging #24311

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 12 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
50 changes: 24 additions & 26 deletions libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]);
}

/*
Expand Down Expand Up @@ -440,33 +430,41 @@ 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();
if (logger == nullptr) {
// 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
}
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down