AP_InertialSensor: parameterise sensor-rate logging, generalise it
AP_InertialSensor: add parameters for push-to-log interval and count AP_InertialSensor: rename BAT_RAW to BAT_OPT This becomes a bitmask of options for the BatchSampler AP_InertialSensor: rename 'fast sample' to 'sensorrate sample' AP_InertialSensor: const sensor-rate filter method AP_InertialSampler: remove hard-coding of sample rate multiplier AP_InertialSensor: use parameter to enable/disable sensor-rate logging AP_InertialSensor: use a parameter to control sensor-rate logging AP_InertialSensor: allow backends to override sensor data multiplier e.g. some accelerometers are sensitive over wider ranges than the default 16G AP_Inertialsensor: correct sample rate multiplier
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
44131202cd
commit
1a640e3405
@ -498,6 +498,7 @@ uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz,
|
||||
|
||||
_gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
|
||||
_gyro_over_sampling[_gyro_count] = 1;
|
||||
_gyro_raw_sampling_multiplier[_gyro_count] = INT16_MAX/radians(2000);
|
||||
|
||||
bool saved = _gyro_id[_gyro_count].load();
|
||||
|
||||
@ -531,6 +532,7 @@ uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz,
|
||||
|
||||
_accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
|
||||
_accel_over_sampling[_accel_count] = 1;
|
||||
_accel_raw_sampling_multiplier[_accel_count] = INT16_MAX/(16*GRAVITY_MSS);
|
||||
|
||||
bool saved = _accel_id[_accel_count].load();
|
||||
|
||||
|
||||
@ -294,18 +294,37 @@ public:
|
||||
// a function called by the main thread at the main loop rate:
|
||||
void periodic();
|
||||
|
||||
bool doing_sensor_rate_logging() const { return _doing_sensor_rate_logging; }
|
||||
|
||||
// class level parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// Parameters
|
||||
AP_Int16 _required_count;
|
||||
AP_Int8 _sensor_mask;
|
||||
AP_Int8 _batch_raw;
|
||||
AP_Int8 _batch_options_mask;
|
||||
|
||||
// Parameters controlling pushing data to DataFlash:
|
||||
// Each DF message is ~ 108 bytes in size, so we use about 1kB/s of
|
||||
// logging bandwidth with a 100ms interval. If we are taking
|
||||
// 1024 samples then we need to send 32 packets, so it will
|
||||
// take ~3 seconds to push a complete batch to the log. If
|
||||
// you are running a on an FMU with three IMUs then you
|
||||
// will loop back around to the first sensor after about
|
||||
// twenty seconds.
|
||||
AP_Int16 samples_per_msg;
|
||||
AP_Int8 push_interval_ms;
|
||||
|
||||
// end Parameters
|
||||
|
||||
private:
|
||||
|
||||
enum batch_opt_t {
|
||||
BATCH_OPT_SENSOR_RATE = (1<<0),
|
||||
};
|
||||
|
||||
void rotate_to_next_sensor();
|
||||
void update_doing_sensor_rate_logging();
|
||||
|
||||
bool should_log(uint8_t instance, IMU_SENSOR_TYPE type);
|
||||
void push_data_to_log();
|
||||
@ -314,6 +333,7 @@ public:
|
||||
|
||||
bool initialised : 1;
|
||||
bool isbh_sent : 1;
|
||||
bool _doing_sensor_rate_logging : 1;
|
||||
uint8_t instance : 3; // instance we are sending data for
|
||||
AP_InertialSensor::IMU_SENSOR_TYPE type : 1;
|
||||
uint16_t isb_seqnum;
|
||||
@ -325,20 +345,7 @@ public:
|
||||
uint32_t last_sent_ms;
|
||||
|
||||
// all samples are multiplied by this
|
||||
static const uint16_t multiplier_accel = INT16_MAX/(16*GRAVITY_MSS);
|
||||
static const uint16_t multiplier_gyro = INT16_MAX/radians(2000);
|
||||
uint16_t multiplier = multiplier_accel;
|
||||
|
||||
// push blocks to DataFlash at regular intervals. each
|
||||
// message is ~ 108 bytes in size, so we use about 1kB/s of
|
||||
// logging bandwidth with a 100ms interval. If we are taking
|
||||
// 1024 samples then we need to send 32 packets, so it will
|
||||
// take ~3 seconds to push a complete batch to the log. If
|
||||
// you are running a on an FMU with three IMUs then you
|
||||
// will loop back around to the first sensor after about
|
||||
// twenty seconds.
|
||||
const uint8_t push_interval_ms = 20;
|
||||
const uint16_t samples_per_msg = 32;
|
||||
uint16_t multiplier; // initialised as part of init()
|
||||
|
||||
const AP_InertialSensor &_imu;
|
||||
};
|
||||
@ -409,6 +416,14 @@ private:
|
||||
Vector3f _last_delta_angle[INS_MAX_INSTANCES];
|
||||
Vector3f _last_raw_gyro[INS_MAX_INSTANCES];
|
||||
|
||||
// bitmask indicating if a sensor is doing sensor-rate sampling:
|
||||
uint8_t _accel_sensor_rate_sampling_enabled;
|
||||
uint8_t _gyro_sensor_rate_sampling_enabled;
|
||||
|
||||
// multipliers for data supplied via sensor-rate logging:
|
||||
uint16_t _accel_raw_sampling_multiplier[INS_MAX_INSTANCES];
|
||||
uint16_t _gyro_raw_sampling_multiplier[INS_MAX_INSTANCES];
|
||||
|
||||
// product id
|
||||
AP_Int16 _old_product_id;
|
||||
|
||||
|
||||
@ -54,7 +54,7 @@ void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t
|
||||
sensor may vary slightly from the system clock. This slowly adjusts
|
||||
the rate to the observed rate
|
||||
*/
|
||||
void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz)
|
||||
void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const
|
||||
{
|
||||
uint32_t now = AP_HAL::micros();
|
||||
if (start_us == 0) {
|
||||
@ -234,7 +234,9 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa
|
||||
};
|
||||
dataflash->WriteBlock(&pkt, sizeof(pkt));
|
||||
} else {
|
||||
//_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us, gyro);
|
||||
if (!_imu.batchsampler.doing_sensor_rate_logging()) {
|
||||
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us, gyro);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -327,13 +329,20 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
|
||||
log_accel_raw(instance, sample_us, accel);
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Backend::_notify_new_accel_fast_sample(uint8_t instance, const Vector3f &accel)
|
||||
void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel)
|
||||
{
|
||||
if (!_imu.batchsampler.doing_sensor_rate_logging()) {
|
||||
return;
|
||||
}
|
||||
|
||||
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, AP_HAL::micros64(), accel);
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Backend::_notify_new_gyro_fast_sample(uint8_t instance, const Vector3f &gyro)
|
||||
void AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro)
|
||||
{
|
||||
if (!_imu.batchsampler.doing_sensor_rate_logging()) {
|
||||
return;
|
||||
}
|
||||
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, AP_HAL::micros64(), gyro);
|
||||
}
|
||||
|
||||
@ -356,7 +365,9 @@ void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t s
|
||||
};
|
||||
dataflash->WriteBlock(&pkt, sizeof(pkt));
|
||||
} else {
|
||||
//_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, sample_us, accel);
|
||||
if (!_imu.batchsampler.doing_sensor_rate_logging()) {
|
||||
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, sample_us, accel);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -105,7 +105,7 @@ public:
|
||||
DEVTYPE_INS_ICM20789 = 0x27,
|
||||
DEVTYPE_INS_ICM20689 = 0x28,
|
||||
};
|
||||
|
||||
|
||||
protected:
|
||||
// access to frontend
|
||||
AP_InertialSensor &_imu;
|
||||
@ -143,9 +143,35 @@ protected:
|
||||
|
||||
// set the amount of oversamping a gyro is doing
|
||||
void _set_gyro_oversampling(uint8_t instance, uint8_t n);
|
||||
|
||||
|
||||
// indicate the backend is doing sensor-rate sampling for this accel
|
||||
void _set_accel_sensor_rate_sampling_enabled(uint8_t instance, bool value) {
|
||||
const uint8_t bit = (1<<instance);
|
||||
if (value) {
|
||||
_imu._accel_sensor_rate_sampling_enabled |= bit;
|
||||
} else {
|
||||
_imu._accel_sensor_rate_sampling_enabled &= ~bit;
|
||||
}
|
||||
}
|
||||
|
||||
void _set_gyro_sensor_rate_sampling_enabled(uint8_t instance, bool value) {
|
||||
const uint8_t bit = (1<<instance);
|
||||
if (value) {
|
||||
_imu._gyro_sensor_rate_sampling_enabled |= bit;
|
||||
} else {
|
||||
_imu._gyro_sensor_rate_sampling_enabled &= ~bit;
|
||||
}
|
||||
}
|
||||
|
||||
void _set_raw_sample_accel_multiplier(uint8_t instance, uint16_t mul) {
|
||||
_imu._accel_raw_sampling_multiplier[instance] = mul;
|
||||
}
|
||||
void _set_raw_sampl_gyro_multiplier(uint8_t instance, uint16_t mul) {
|
||||
_imu._gyro_raw_sampling_multiplier[instance] = mul;
|
||||
}
|
||||
|
||||
// update the sensor rate for FIFO sensors
|
||||
void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz);
|
||||
void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const;
|
||||
|
||||
// set accelerometer max absolute offset for calibration
|
||||
void _set_accel_max_abs_offset(uint8_t instance, float offset);
|
||||
@ -226,8 +252,10 @@ protected:
|
||||
return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
|
||||
}
|
||||
|
||||
void _notify_new_accel_fast_sample(uint8_t instance, const Vector3f &accel);
|
||||
void _notify_new_gyro_fast_sample(uint8_t instance, const Vector3f &gyro);
|
||||
// called by subclass when data is received from the sensor, thus
|
||||
// at the 'sensor rate'
|
||||
void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel);
|
||||
void _notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro);
|
||||
|
||||
/*
|
||||
notify of a FIFO reset so we don't use bad data to update observed sensor rate
|
||||
|
||||
@ -262,7 +262,11 @@ void AP_InertialSensor_Invensense::start()
|
||||
// update backend sample rate
|
||||
_set_accel_raw_sample_rate(_accel_instance, _backend_rate_hz);
|
||||
_set_gyro_raw_sample_rate(_gyro_instance, _backend_rate_hz);
|
||||
|
||||
|
||||
// indicate what multiplier is appropriate for the sensors'
|
||||
// readings to fit them into an int16_t:
|
||||
_set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
|
||||
|
||||
if (_fast_sampling) {
|
||||
hal.console->printf("MPU[%u]: enabled fast sampling rate %uHz/%uHz\n",
|
||||
_accel_instance, _backend_rate_hz*_fifo_downsample_rate, _backend_rate_hz);
|
||||
@ -445,7 +449,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
|
||||
gives very good aliasing rejection at frequencies well above what
|
||||
can be handled with 1kHz sample rates.
|
||||
*/
|
||||
bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
|
||||
bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples)
|
||||
{
|
||||
int32_t tsum = 0;
|
||||
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale;
|
||||
@ -477,7 +481,7 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u
|
||||
}
|
||||
_accum.accel += _accum.accel_filter.apply(a);
|
||||
Vector3f a2 = a * _accel_scale;
|
||||
_notify_new_accel_fast_sample(_accel_instance, a2);
|
||||
_notify_new_accel_sensor_rate_sample(_accel_instance, a2);
|
||||
}
|
||||
|
||||
Vector3f g(int16_val(data, 5),
|
||||
@ -485,7 +489,7 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u
|
||||
-int16_val(data, 6));
|
||||
|
||||
Vector3f g2 = g * GYRO_SCALE;
|
||||
_notify_new_gyro_fast_sample(_gyro_instance, g2);
|
||||
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
|
||||
|
||||
_accum.gyro += _accum.gyro_filter.apply(g);
|
||||
_accum.count++;
|
||||
@ -583,7 +587,7 @@ void AP_InertialSensor_Invensense::_read_fifo()
|
||||
}
|
||||
|
||||
if (_fast_sampling) {
|
||||
if (!_accumulate_fast_sampling(rx, n)) {
|
||||
if (!_accumulate_sensor_rate_sampling(rx, n)) {
|
||||
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE);
|
||||
break;
|
||||
}
|
||||
@ -679,6 +683,9 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
|
||||
_set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2);
|
||||
_set_gyro_oversampling(_gyro_instance, _fifo_downsample_rate);
|
||||
|
||||
_set_accel_sensor_rate_sampling_enabled(_accel_instance, true);
|
||||
_set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true);
|
||||
|
||||
/* set divider for internal sample rate to 0x1F when fast
|
||||
sampling enabled. This reduces the impact of the slave
|
||||
sensor on the sample rate. It ends up with around 75Hz
|
||||
|
||||
@ -65,7 +65,12 @@ public:
|
||||
Invensense_ICM20789,
|
||||
Invensense_ICM20689,
|
||||
};
|
||||
|
||||
|
||||
// acclerometers on Invensense sensors will return values up to
|
||||
// 24G, but they are not guaranteed to be remotely linear past
|
||||
// 16G
|
||||
const uint16_t multiplier_accel = INT16_MAX/(26*GRAVITY_MSS);
|
||||
|
||||
private:
|
||||
AP_InertialSensor_Invensense(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
@ -96,7 +101,7 @@ private:
|
||||
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
|
||||
|
||||
bool _accumulate(uint8_t *samples, uint8_t n_samples);
|
||||
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
|
||||
bool _accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples);
|
||||
|
||||
bool _check_raw_temp(int16_t t2);
|
||||
|
||||
@ -141,8 +146,8 @@ private:
|
||||
uint8_t *_fifo_buffer;
|
||||
|
||||
/*
|
||||
accumulators for fast sampling
|
||||
See description in _accumulate_fast_sampling()
|
||||
accumulators for sensor_rate sampling
|
||||
See description in _accumulate_sensor_rate_sampling()
|
||||
*/
|
||||
struct {
|
||||
Vector3f accel;
|
||||
|
||||
@ -430,14 +430,14 @@ bool AP_InertialSensor_Revo::_accumulate(uint8_t *samples, uint8_t n_samples)
|
||||
}
|
||||
|
||||
/*
|
||||
when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate.
|
||||
when doing sensor-rate sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate.
|
||||
|
||||
To filter this we first apply a 1p low pass filter at 188Hz, then we
|
||||
average over 8 samples to bring the data rate down to 1kHz. This
|
||||
gives very good aliasing rejection at frequencies well above what
|
||||
can be handled with 1kHz sample rates.
|
||||
*/
|
||||
bool AP_InertialSensor_Revo::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
|
||||
bool AP_InertialSensor_Revo::_accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples)
|
||||
{
|
||||
int32_t tsum = 0;
|
||||
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale;
|
||||
@ -557,7 +557,7 @@ void AP_InertialSensor_Revo::_read_fifo()
|
||||
|
||||
|
||||
if (_fast_sampling) {
|
||||
if (!_accumulate_fast_sampling(rx, 1)) {
|
||||
if (!_accumulate_sensor_rate_sampling(rx, 1)) {
|
||||
// debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);
|
||||
// break; don't break before all items in queue will be readed
|
||||
continue;
|
||||
|
||||
@ -109,7 +109,7 @@ private:
|
||||
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
|
||||
|
||||
bool _accumulate(uint8_t *samples, uint8_t n_samples);
|
||||
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
|
||||
bool _accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples);
|
||||
|
||||
bool _check_raw_temp(int16_t t2);
|
||||
|
||||
@ -149,7 +149,7 @@ private:
|
||||
|
||||
/*
|
||||
accumulators for fast sampling
|
||||
See description in _accumulate_fast_sampling()
|
||||
See description in _accumulate_sensor_rate_sampling()
|
||||
*/
|
||||
struct {
|
||||
Vector3f accel;
|
||||
|
||||
@ -18,13 +18,26 @@ const AP_Param::GroupInfo AP_InertialSensor::BatchSampler::var_info[] = {
|
||||
// @Bitmask: 0:IMU1,1:IMU2,2:IMU3
|
||||
AP_GROUPINFO("BAT_MASK", 2, AP_InertialSensor::BatchSampler, _sensor_mask, DEFAULT_IMU_LOG_BAT_MASK),
|
||||
|
||||
// @Param: BAT_RAW
|
||||
// @DisplayName: Enable raw batch logging
|
||||
// @Description: This allows 4kHz IMU logging on supported sensors
|
||||
// @Param: BAT_OPT
|
||||
// @DisplayName: Batch Logging Options Mask
|
||||
// @Description: Options for the BatchSampler
|
||||
// @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP)
|
||||
// @User: Advanced
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
AP_GROUPINFO("BAT_RAW", 3, AP_InertialSensor::BatchSampler, _batch_raw, 0),
|
||||
|
||||
AP_GROUPINFO("BAT_OPT", 3, AP_InertialSensor::BatchSampler, _batch_options_mask, 0),
|
||||
|
||||
// @Param: BAT_LGIN
|
||||
// @DisplayName: logging interval
|
||||
// @Description: Interval between pushing samples to the DataFlash log
|
||||
// @Units: ms
|
||||
// @Increment: 10
|
||||
AP_GROUPINFO("BAT_LGIN", 4, AP_InertialSensor::BatchSampler, push_interval_ms, 20),
|
||||
|
||||
// @Param: BAT_LGCT
|
||||
// @DisplayName: logging count
|
||||
// @Description: Number of samples to push to count every @PREFIX@BAT_LGIN
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("BAT_LGCT", 5, AP_InertialSensor::BatchSampler, samples_per_msg, 32),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -71,6 +84,23 @@ void AP_InertialSensor::BatchSampler::periodic()
|
||||
push_data_to_log();
|
||||
}
|
||||
|
||||
void AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging()
|
||||
{
|
||||
if (!((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_SENSOR_RATE)) {
|
||||
_doing_sensor_rate_logging = false;
|
||||
return;
|
||||
}
|
||||
const uint8_t bit = (1<<instance);
|
||||
switch (type) {
|
||||
case IMU_SENSOR_TYPE_GYRO:
|
||||
_doing_sensor_rate_logging = _imu._gyro_sensor_rate_sampling_enabled & bit;
|
||||
break;
|
||||
case IMU_SENSOR_TYPE_ACCEL:
|
||||
_doing_sensor_rate_logging = _imu._accel_sensor_rate_sampling_enabled & bit;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
|
||||
{
|
||||
if (_sensor_mask == 0) {
|
||||
@ -85,13 +115,13 @@ void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
|
||||
if (type == IMU_SENSOR_TYPE_ACCEL) {
|
||||
// we have logged accelerometers, now log gyros:
|
||||
type = IMU_SENSOR_TYPE_GYRO;
|
||||
multiplier = multiplier_gyro;
|
||||
multiplier = _imu._gyro_raw_sampling_multiplier[instance];
|
||||
update_doing_sensor_rate_logging();
|
||||
return;
|
||||
}
|
||||
|
||||
// log for accel sensor:
|
||||
type = IMU_SENSOR_TYPE_ACCEL;
|
||||
multiplier = multiplier_accel;
|
||||
|
||||
// move to next IMU backend:
|
||||
|
||||
@ -100,18 +130,34 @@ void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
|
||||
const uint8_t _count = MIN(_imu._accel_count, _imu._gyro_count);
|
||||
|
||||
// find next backend instance to log:
|
||||
bool haveinstance = false;
|
||||
for (uint8_t i=instance+1; i<_count; i++) {
|
||||
if (_sensor_mask & (1U<<i)) {
|
||||
instance = i;
|
||||
return;
|
||||
haveinstance = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (uint8_t i=0; i<instance; i++) {
|
||||
if (_sensor_mask & (1U<<i)) {
|
||||
instance = i;
|
||||
return;
|
||||
if (!haveinstance) {
|
||||
for (uint8_t i=0; i<=instance; i++) {
|
||||
if (_sensor_mask & (1U<<i)) {
|
||||
instance = i;
|
||||
haveinstance = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!haveinstance) {
|
||||
// should not happen!
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
abort();
|
||||
#endif
|
||||
instance = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
multiplier = _imu._accel_raw_sampling_multiplier[instance];
|
||||
update_doing_sensor_rate_logging();
|
||||
}
|
||||
|
||||
void AP_InertialSensor::BatchSampler::push_data_to_log()
|
||||
@ -126,8 +172,7 @@ void AP_InertialSensor::BatchSampler::push_data_to_log()
|
||||
// insuffucient data to pack a packet
|
||||
return;
|
||||
}
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
if (now - last_sent_ms < push_interval_ms) {
|
||||
if (AP_HAL::millis() - last_sent_ms < (uint16_t)push_interval_ms) {
|
||||
// avoid flooding DataFlash's buffer
|
||||
return;
|
||||
}
|
||||
@ -142,10 +187,16 @@ void AP_InertialSensor::BatchSampler::push_data_to_log()
|
||||
float sample_rate = 0; // avoid warning about uninitialised values
|
||||
switch(type) {
|
||||
case IMU_SENSOR_TYPE_GYRO:
|
||||
sample_rate = _imu._gyro_raw_sample_rates[instance] * 8;
|
||||
sample_rate = _imu._gyro_raw_sample_rates[instance];
|
||||
if (_doing_sensor_rate_logging) {
|
||||
sample_rate *= _imu._gyro_over_sampling[instance];
|
||||
}
|
||||
break;
|
||||
case IMU_SENSOR_TYPE_ACCEL:
|
||||
sample_rate = _imu._accel_raw_sample_rates[instance] * 4;
|
||||
sample_rate = _imu._accel_raw_sample_rates[instance];
|
||||
if (_doing_sensor_rate_logging) {
|
||||
sample_rate *= _imu._accel_over_sampling[instance];
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (!dataflash->Log_Write_ISBH(isb_seqnum,
|
||||
@ -200,6 +251,9 @@ bool AP_InertialSensor::BatchSampler::should_log(uint8_t _instance, IMU_SENSOR_T
|
||||
return false;
|
||||
}
|
||||
DataFlash_Class *dataflash = DataFlash_Class::instance();
|
||||
if (dataflash == nullptr) {
|
||||
return false;
|
||||
}
|
||||
#define MASK_LOG_ANY 0xFFFF
|
||||
if (!dataflash->should_log(MASK_LOG_ANY)) {
|
||||
return false;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user