From 8c2d3945defa92ae4d93d4697fb504c4a6e10bc3 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 11 Mar 2019 18:26:40 +0530 Subject: [PATCH] AP_IneertialSensor: add support for custom accel scale adds support for 30g measurement on ICM20649 --- .../AP_InertialSensor/AP_InertialSensor.cpp | 6 ++--- .../AP_InertialSensor/AP_InertialSensor.h | 1 - .../AP_InertialSensor_Backend.h | 6 +++++ .../AP_InertialSensor_Invensense.cpp | 8 +++--- .../AP_InertialSensor_Invensensev2.cpp | 27 ++++++++++--------- .../AP_InertialSensor_Invensensev2.h | 2 +- 6 files changed, 28 insertions(+), 22 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index db3254d4e0..b9d2aa0b67 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1613,9 +1613,9 @@ AuxiliaryBus *AP_InertialSensor::get_auxiliary_bus(int16_t backend_id, uint8_t i void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt) { // check for clipping - if (fabsf(accel.x) > AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS || - fabsf(accel.y) > AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS || - fabsf(accel.z) > AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS) { + if (fabsf(accel.x) > _backends[instance]->get_clip_limit() || + fabsf(accel.y) > _backends[instance]->get_clip_limit() || + fabsf(accel.z) > _backends[instance]->get_clip_limit()) { _accel_clip_count[instance]++; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 9bbc941538..6d4ea2209d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -3,7 +3,6 @@ // Gyro and Accelerometer calibration criteria #define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f #define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0f -#define AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS (15.5f*GRAVITY_MSS) // accelerometer values over 15.5G are recorded as a clipping error #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ 5.0f // accel vibration floor filter hz #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 2af7a6c21d..62affba904 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -70,6 +70,9 @@ public: */ int16_t get_id() const { return _id; } + //Returns the Clip Limit + float get_clip_limit() const { return _clip_limit; } + // notify of a fifo reset void notify_fifo_reset(void); @@ -113,6 +116,9 @@ protected: // semaphore for access to shared frontend data HAL_Semaphore_Recursive _sem; + //Default Clip Limit + float _clip_limit = 15.5f * GRAVITY_MSS; + void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel); void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index f4c2d83308..88d66442b5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -453,7 +453,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl 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; + const int32_t unscaled_clip_limit = _clip_limit / _accel_scale; bool clipped = false; bool ret = true; @@ -475,9 +475,9 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam Vector3f a(int16_val(data, 1), int16_val(data, 0), -int16_val(data, 2)); - if (fabsf(a.x) > clip_limit || - fabsf(a.y) > clip_limit || - fabsf(a.z) > clip_limit) { + if (fabsf(a.x) > unscaled_clip_limit || + fabsf(a.y) > unscaled_clip_limit || + fabsf(a.z) > unscaled_clip_limit) { clipped = true; } _accum.accel += _accum.accel_filter.apply(a); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index 3c206c6a97..aeb429c057 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -40,11 +40,6 @@ extern const AP_HAL::HAL& hal; */ static const float GYRO_SCALE = (0.0174532f / 16.4f); -/* - * DS-000189-ICM-20948-v1.3.pdf, page 12, section 3.2 lists LSB sensitivity of - * accel as 2048 LSB/DPS at scale factor of +/- 16g (FS_SEL==3) - */ -static const float ACCEL_SCALE = (GRAVITY_MSS / 2048.f); /* EXT_SYNC allows for frame synchronisation with an external device such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit @@ -180,14 +175,17 @@ void AP_InertialSensor_Invensensev2::start() case Invensensev2_ICM20948: gdev = DEVTYPE_INS_ICM20948; adev = DEVTYPE_INS_ICM20948; + _accel_scale = (GRAVITY_MSS / 2048.f); break; case Invensensev2_ICM20648: gdev = DEVTYPE_INS_ICM20648; adev = DEVTYPE_INS_ICM20648; + _accel_scale = (GRAVITY_MSS / 2048.f); break; case Invensensev2_ICM20649: gdev = DEVTYPE_INS_ICM20649; adev = DEVTYPE_INS_ICM20649; + _accel_scale = (GRAVITY_MSS / 1024.f); break; default: gdev = DEVTYPE_INS_ICM20948; @@ -234,7 +232,7 @@ void AP_InertialSensor_Invensensev2::start() set_accel_orientation(_accel_instance, _rotation); // setup scale factors for fifo data after downsampling - _fifo_accel_scale = ACCEL_SCALE / (MAX(_fifo_downsample_rate,2)/2); + _fifo_accel_scale = _accel_scale / (MAX(_fifo_downsample_rate,2)/2); _fifo_gyro_scale = GYRO_SCALE / _fifo_downsample_rate; // allocate fifo buffer @@ -319,7 +317,7 @@ bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_sam accel = Vector3f(int16_val(data, 1), int16_val(data, 0), -int16_val(data, 2)); - accel *= ACCEL_SCALE; + accel *= _accel_scale; int16_t t2 = int16_val(data, 6); if (!_check_raw_temp(t2)) { @@ -356,10 +354,10 @@ bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_sam bool AP_InertialSensor_Invensensev2::_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; + int32_t unscaled_clip_limit = _clip_limit / _accel_scale; bool clipped = false; bool ret = true; - + for (uint8_t i = 0; i < n_samples; i++) { const uint8_t *data = samples + INV2_SAMPLE_SIZE * i; @@ -377,13 +375,13 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s Vector3f a(int16_val(data, 1), int16_val(data, 0), -int16_val(data, 2)); - if (fabsf(a.x) > clip_limit || - fabsf(a.y) > clip_limit || - fabsf(a.z) > clip_limit) { + if (fabsf(a.x) > unscaled_clip_limit || + fabsf(a.y) > unscaled_clip_limit || + fabsf(a.z) > unscaled_clip_limit) { clipped = true; } _accum.accel += _accum.accel_filter.apply(a); - Vector3f a2 = a * ACCEL_SCALE; + Vector3f a2 = a * _accel_scale; _notify_new_accel_sensor_rate_sample(_accel_instance, a2); } @@ -704,6 +702,9 @@ bool AP_InertialSensor_Invensensev2::_hardware_init(void) return false; } + if (_inv2_type == Invensensev2_ICM20649) { + _clip_limit = 29.5f * GRAVITY_MSS; + } _dev->get_semaphore()->give(); return true; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h index 7392248180..55cf57396e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h @@ -108,7 +108,7 @@ private: float temp_zero = 21; // degC float _temp_filtered; - + float _accel_scale; float _fifo_accel_scale; float _fifo_gyro_scale; LowPassFilter2pFloat _temp_filter;