aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: add INS_RAW_LOG_OPT to allow raw logging of post, and pre+post on primary or all gyros

Browse Source
This commit is contained in:
Iampete1 2023-09-29 22:26:31 +01:00 committed by Andrew Tridgell
parent 3013a7959d
commit 3429276224
4 changed files with 44 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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

12
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -766,6 +766,18 @@ private:
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 {

50
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -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
}

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -315,7 +315,7 @@ private:
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