aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed accel clip detection with fast sampling

Browse Source 
we need to check on every sample at the full rate
This commit is contained in:
Andrew Tridgell 2016-11-10 13:39:17 +11:00
parent 0eac781559
commit af0ec41652
3 changed files with 38 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

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

20
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

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

20
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

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