Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: allow concurrent logging of both pre- and post-filter IMU data for FFT 

remove batch logging bitfields
This commit is contained in:
Andy Piper 2022-06-25 21:16:55 +01:00 committed by Andrew Tridgell
parent 2318c0e505
commit 9d851a0c1a
3 changed files with 31 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

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

7
libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
View File

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

18
libraries/AP_InertialSensor/BatchSampler.cpp
View File

@ -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<<instance) > (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<<i)) {
instance = i;
haveinstance = true;
post_filter = !post_filter;
break;
}
}
@ -164,6 +165,7 @@ void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
abort();
#endif
instance = 0;
post_filter = false;
return;
}