Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IneertialSensor: add support for custom accel scale

Browse Source 
adds support for 30g measurement on ICM20649
This commit is contained in:
Siddharth Purohit 2019-03-11 18:26:40 +05:30 committed by Andrew Tridgell
parent 3a833d9a90
commit 8c2d3945de
6 changed files with 28 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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]++;
}

1
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

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

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

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

8
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

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

27
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

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

2
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h
View File

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