From 1a640e34057489be0f8293198cd28e93fea19161 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 19 Mar 2018 10:28:33 +1100 Subject: [PATCH] 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 --- .../AP_InertialSensor/AP_InertialSensor.cpp | 2 + .../AP_InertialSensor/AP_InertialSensor.h | 45 ++++++---- .../AP_InertialSensor_Backend.cpp | 21 +++-- .../AP_InertialSensor_Backend.h | 38 ++++++-- .../AP_InertialSensor_Invensense.cpp | 17 ++-- .../AP_InertialSensor_Invensense.h | 13 ++- .../AP_InertialSensor_Revo.cpp | 6 +- .../AP_InertialSensor_Revo.h | 4 +- libraries/AP_InertialSensor/BatchSampler.cpp | 88 +++++++++++++++---- 9 files changed, 178 insertions(+), 56 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 532b942be0..359b5db432 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -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(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 3a2e739944..5f8615a8a8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 3f5b99a9ae..cd763270de 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -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); + } } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 4cff5140b5..804e408494 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -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<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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 4d80129821..bdc14a4c5e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -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 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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Revo.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Revo.cpp index 6eaa1fea33..8ce6d89e68 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Revo.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Revo.cpp @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Revo.h b/libraries/AP_InertialSensor/AP_InertialSensor_Revo.h index 8d06f3ee33..1a3471f61c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Revo.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Revo.h @@ -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; diff --git a/libraries/AP_InertialSensor/BatchSampler.cpp b/libraries/AP_InertialSensor/BatchSampler.cpp index 3bc954953e..1eae46cb9a 100644 --- a/libraries/AP_InertialSensor/BatchSampler.cpp +++ b/libraries/AP_InertialSensor/BatchSampler.cpp @@ -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<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;