Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-02 10:23:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: tier Invensensev3 gyro bias override by variant

Browse Source 
The v3 driver covers eight chips with a wide spread in ZRO
temperature coefficient. ArduPilot's startup gyro cal removes the
static ZRO, so the EKF clamp must bound the in-flight drift driven
mostly by temperature swing times the temp coefficient.

  ICM-42688-P, ICM-45686:  0.005 deg/s/C -> ~0.3 deg/s over 60C
  ICM-40609-D:             0.01  deg/s/C
  ICM-42670-P:             0.015 deg/s/C
  ICM-42605, IIM-42652:    0.02  deg/s/C -> ~1.2 deg/s over 60C
  IIM-42653:               0.04  deg/s/C -> ~2.4 deg/s over 60C

Applying radians(2.0) across the whole family was too tight for
IIM-42653 and marginal for the mid-tier parts. Tier the override:
the two very-low-drift parts keep radians(2.0)/1.0 deg/s, the
mid-tier parts get radians(3.0)/1.5 deg/s, and ICM-40605 (no
datasheet checked) plus IIM-42653 fall back to the base default.

Datasheets consulted: DS-000347, DS-000489, DS-000292, DS-000330,
DS-000451, DS-000440, DS-000529.
This commit is contained in:
Andy Piper
2026-05-13 18:29:03 +10:00
committed by Peter Barker
parent 9b16c49a96
commit 1be84b6dde
2 changed files with 55 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
+37
View File
@@ -256,6 +256,43 @@ void AP_InertialSensor_Invensensev3::fifo_reset()
notify_gyro_fifo_reset(gyro_instance);
}
// see header for the per-variant rationale and datasheet references
float AP_InertialSensor_Invensensev3::gyro_bias_limit_rads() const
{
switch (inv3_type) {
case Invensensev3_Type::ICM42688:
case Invensensev3_Type::ICM45686:
return radians(2.0f);
case Invensensev3_Type::ICM42605:
case Invensensev3_Type::ICM40609:
case Invensensev3_Type::ICM42670:
case Invensensev3_Type::IIM42652:
return radians(3.0f);
case Invensensev3_Type::ICM40605:
case Invensensev3_Type::IIM42653:
break;
}
return AP_InertialSensor_Backend::gyro_bias_limit_rads();
}
float AP_InertialSensor_Invensensev3::gyro_bias_init_dps() const
{
switch (inv3_type) {
case Invensensev3_Type::ICM42688:
case Invensensev3_Type::ICM45686:
return 1.0f;
case Invensensev3_Type::ICM42605:
case Invensensev3_Type::ICM40609:
case Invensensev3_Type::ICM42670:
case Invensensev3_Type::IIM42652:
return 1.5f;
case Invensensev3_Type::ICM40605:
case Invensensev3_Type::IIM42653:
break;
}
return AP_InertialSensor_Backend::gyro_bias_init_dps();
}
void AP_InertialSensor_Invensensev3::start()
{
// pre-fetch instance numbers for checking fast sampling settings
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
+18 -14
View File
@@ -83,20 +83,24 @@ private:
return backend_rate_hz;
}
// The EKF clamps and the initial covariance below act on the
// residual gyro bias after ArduPilot's startup calibration has
// removed the static offset. Across the v3 family (ICM-42605,
// ICM-42688, ICM-40605, ICM-40609, ICM-42670, ICM-45686, IIM-42652,
// IIM-42653), residual drift over a flight is dominated by bias
// instability plus temperature drift and is well under 1 deg/s, so
// a 2 deg/s clamp and 1 deg/s init uncertainty are conservative for
// all variants.
float gyro_bias_limit_rads() const override {
return radians(2.0f); // 0.035 rad/s
}
float gyro_bias_init_dps() const override {
return 1.0f;
}
// The EKF clamp acts on residual gyro bias after ArduPilot's
// startup calibration has removed the static ZRO. Residual drift
// over a flight is therefore dominated by ZRO temperature drift
// (temp coefficient x in-flight temperature swing). The v3 family
// has a wide spread in this coefficient, so the override is tiered:
//
// tier parts ZRO drift limit / init
// ---- ----------------------- ---------- ------------------
// low ICM-42688-P, ICM-45686 0.005/C radians(2.0) / 1.0
// mid ICM-42605, ICM-40609-D, 0.01-0.02/C radians(3.0) / 1.5
// ICM-42670-P, IIM-42652
// none ICM-40605, IIM-42653, 0.04/C or base default
// (anything else) unknown (0.5 rad/s / 2.5)
//
// See datasheets DS-000347, DS-000489, DS-000292, DS-000330,
// DS-000451, DS-000440, DS-000529.
float gyro_bias_limit_rads() const override;
float gyro_bias_init_dps() const override;
// reset FIFO configure1 register
uint8_t fifo_config1;