From d71278f1bcc63c73b756ede24ebacb66eddb9c0d Mon Sep 17 00:00:00 2001 From: muramura Date: Sun, 30 Jun 2024 09:11:50 +0900 Subject: [PATCH] AP_InertialSensor: Clearly state the maximum G-force --- .../AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp | 7 ++++--- libraries/AP_InertialSensor/AP_InertialSensor_Backend.h | 2 +- .../AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp index e407d6da4d474..ddfc05eac251f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp @@ -137,7 +137,7 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(uint16_t &prod_id) // can do up to 40G opmode = OpMode::Basic; accel_scale = 1.25 * GRAVITY_MSS * 0.001; - _clip_limit = 39.5f * GRAVITY_MSS; + _clip_limit = (40.0f - 0.5f) * GRAVITY_MSS; gyro_scale = radians(0.1); expected_sample_rate_hz = 2000; return true; @@ -146,7 +146,7 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(uint16_t &prod_id) // can do up to 40G opmode = OpMode::Basic; accel_scale = 1.25 * GRAVITY_MSS * 0.001; - _clip_limit = 39.5f * GRAVITY_MSS; + _clip_limit = (40.0f - 0.5f) * GRAVITY_MSS; expected_sample_rate_hz = 2000; // RANG_MDL register used for gyro range uint16_t rang_mdl = read_reg16(REG_RANG_MDL); @@ -167,11 +167,12 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(uint16_t &prod_id) } case PROD_ID_16507: { + // can do up to 40G opmode = OpMode::Delta32; expected_sample_rate_hz = 1200; accel_scale = 392.0 / 2097152000.0; dvel_scale = 400.0 / 0x7FFFFFFF; - _clip_limit = 39.5f * GRAVITY_MSS; + _clip_limit = (40.0f - 0.5f) * GRAVITY_MSS; // RANG_MDL register used for gyro range uint16_t rang_mdl = read_reg16(REG_RANG_MDL); switch ((rang_mdl >> 2) & 3) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index b86b9bb57fe15..f9eb29bf0f9c2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -139,7 +139,7 @@ class AP_InertialSensor_Backend HAL_Semaphore _sem; //Default Clip Limit - float _clip_limit = 15.5f * GRAVITY_MSS; + float _clip_limit = (16.0f - 0.5f) * GRAVITY_MSS; // instance numbers of accel and gyro data uint8_t gyro_instance; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 9a27617197528..474819cbdf25e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -1037,7 +1037,7 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void) case Invensensev3_Type::ICM42605: case Invensensev3_Type::ICM40605: case Invensensev3_Type::ICM42670: - _clip_limit = 15.5f * GRAVITY_MSS; + _clip_limit = (16.0f - 0.5f) * GRAVITY_MSS; break; }