diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 0bb6e1ecb4..2138dd33e3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -174,6 +174,12 @@ protected: void set_accel_orientation(uint8_t instance, enum Rotation rotation) { _imu._accel_orientation[instance] = rotation; } + + // increment clipping counted. Used by drivers that do decimation before supplying + // samples to the frontend + void increment_clip_count(uint8_t instance) { + _imu._accel_clip_count[instance]++; + } // note that each backend is also expected to have a static detect() // function which instantiates an instance of the backend sensor diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 507521d175..4cf8365f97 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -514,12 +514,20 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint { Vector3l asum, gsum; float tsum = 0; - + const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale; + bool clipped = false; + for (uint8_t i = 0; i < n_samples; i++) { uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i; - asum += Vector3l(int16_val(data, 1), - int16_val(data, 0), - -int16_val(data, 2)); + Vector3l a(int16_val(data, 1), + int16_val(data, 0), + -int16_val(data, 2)); + if (abs(a.x) > clip_limit || + abs(a.y) > clip_limit || + abs(a.z) > clip_limit) { + clipped = true; + } + asum += a; gsum += Vector3l(int16_val(data, 5), int16_val(data, 4), -int16_val(data, 6)); @@ -530,6 +538,10 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint _last_temp = temp; } + if (clipped) { + increment_clip_count(_accel_instance); + } + float ascale = _accel_scale / n_samples; Vector3f accel(asum.x*ascale, asum.y*ascale, asum.z*ascale); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 15cf78f528..703396384f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -443,12 +443,20 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint { Vector3l asum, gsum; float tsum = 0; - + const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / MPU9250_ACCEL_SCALE_1G; + bool clipped = false; + for (uint8_t i = 0; i < n_samples; i++) { uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i; - asum += Vector3l(int16_val(data, 1), - int16_val(data, 0), - -int16_val(data, 2)); + Vector3l a(int16_val(data, 1), + int16_val(data, 0), + -int16_val(data, 2)); + if (abs(a.x) > clip_limit || + abs(a.y) > clip_limit || + abs(a.z) > clip_limit) { + clipped = true; + } + asum += a; gsum += Vector3l(int16_val(data, 5), int16_val(data, 4), -int16_val(data, 6)); @@ -459,6 +467,10 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint _last_temp = temp; } + if (clipped) { + increment_clip_count(_accel_instance); + } + float ascale = MPU9250_ACCEL_SCALE_1G / n_samples; Vector3f accel(asum.x*ascale, asum.y*ascale, asum.z*ascale);