From 8413ab2bf2ca9d8b60d62af73727baaaeed2be4e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 10 Nov 2024 14:03:50 -0900 Subject: [PATCH] AP_InertialSensor: added IIM42653 support --- .../AP_InertialSensor/AP_InertialSensor_Backend.h | 1 + .../AP_InertialSensor_Invensensev3.cpp | 13 +++++++++++++ .../AP_InertialSensor_Invensensev3.h | 1 + 3 files changed, 15 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 00d149e149..817cb2e947 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -136,6 +136,7 @@ public: DEVTYPE_INS_ICM42670 = 0x3A, DEVTYPE_INS_ICM45686 = 0x3B, DEVTYPE_INS_SCHA63T = 0x3C, + DEVTYPE_INS_IIM42653 = 0x3D, }; protected: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index daf54ed64e..d22a7a224f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -136,6 +136,7 @@ extern const AP_HAL::HAL& hal; #define INV3_ID_ICM42605 0x42 #define INV3_ID_ICM42688 0x47 #define INV3_ID_IIM42652 0x6f +#define INV3_ID_IIM42653 0x56 #define INV3_ID_ICM42670 0x67 #define INV3_ID_ICM45686 0xE9 @@ -258,6 +259,12 @@ void AP_InertialSensor_Invensensev3::start() devtype = DEVTYPE_INS_IIM42652; temp_sensitivity = 1.0 / 2.07; break; + case Invensensev3_Type::IIM42653: + devtype = DEVTYPE_INS_IIM42653; + temp_sensitivity = 1.0 / 2.07; + gyro_scale = GYRO_SCALE_4000DPS; + accel_scale = ACCEL_SCALE_32G; + break; case Invensensev3_Type::ICM42688: devtype = DEVTYPE_INS_ICM42688; temp_sensitivity = 1.0 / 2.07; @@ -294,6 +301,7 @@ void AP_InertialSensor_Invensensev3::start() switch (inv3_type) { case Invensensev3_Type::ICM42688: // HiRes 19bit case Invensensev3_Type::IIM42652: // HiRes 19bit + case Invensensev3_Type::IIM42653: // HiRes 19bit case Invensensev3_Type::ICM45686: // HiRes 20bit highres_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; break; @@ -824,6 +832,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) case Invensensev3_Type::ICM42688: case Invensensev3_Type::ICM42605: case Invensensev3_Type::IIM42652: + case Invensensev3_Type::IIM42653: case Invensensev3_Type::ICM42670: { /* fix for the "stuck gyro" issue, which affects all IxM42xxx @@ -967,6 +976,9 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void) case INV3_ID_IIM42652: inv3_type = Invensensev3_Type::IIM42652; return true; + case INV3_ID_IIM42653: + inv3_type = Invensensev3_Type::IIM42653; + return true; case INV3_ID_ICM42670: inv3_type = Invensensev3_Type::ICM42670; return true; @@ -1042,6 +1054,7 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void) switch (inv3_type) { case Invensensev3_Type::ICM45686: case Invensensev3_Type::ICM40609: + case Invensensev3_Type::IIM42653: _clip_limit = 29.5f * GRAVITY_MSS; break; case Invensensev3_Type::ICM42688: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index b3946aa81d..6b4fc49a8c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -37,6 +37,7 @@ public: ICM42605, // No HiRes ICM40605, // No HiRes IIM42652, // HiRes 19bit + IIM42653, // HiRes 19bit ICM42670, // HiRes 19bit ICM45686 // HiRes 20bit };