From a3afa3fe7bd2fa2ea6d386d7ddd96db7ccc9467a Mon Sep 17 00:00:00 2001
From: megazar <48863881+ultrazar@users.noreply.github.com>
Date: Wed, 23 Oct 2024 20:50:00 +0200
Subject: [PATCH] Added IMUTemperature read function for ICM42605

---
 src/main/drivers/accgyro/accgyro_icm42605.c | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c
index 13bc22fcee7..d0c4e3490aa 100644
--- a/src/main/drivers/accgyro/accgyro_icm42605.c
+++ b/src/main/drivers/accgyro/accgyro_icm42605.c
@@ -62,6 +62,7 @@
 
 #define ICM42605_RA_GYRO_DATA_X1                    0x25
 #define ICM42605_RA_ACCEL_DATA_X1                   0x1F
+#define ICM42605_RA_TEMP_DATA1                      0x1D
 
 #define ICM42605_RA_INT_CONFIG                      0x14
 #define ICM42605_INT1_MODE_PULSED                   (0 << 2)
@@ -321,6 +322,20 @@ static bool icm42605GyroRead(gyroDev_t *gyro)
     return true;
 }
 
+static bool icm42605ReadTemperature(gyroDev_t *gyro, int16_t * temp)
+{
+    uint8_t data[2];
+
+    const bool ack = busReadBuf(gyro->busDev, ICM42605_RA_TEMP_DATA1, data, 2);
+    if (!ack) {
+        return false;
+    }
+    // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 132.48) + 25 
+    *temp = ( int16_val_big_endian(data, 0) / 13.248 ) + 250; // Temperature stored as degC*10
+
+    return true;
+}
+
 bool icm42605GyroDetect(gyroDev_t *gyro)
 {
     gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU);
@@ -340,7 +355,7 @@ bool icm42605GyroDetect(gyroDev_t *gyro)
     gyro->initFn = icm42605AccAndGyroInit;
     gyro->readFn = icm42605GyroRead;
     gyro->intStatusFn = gyroCheckDataReady;
-    gyro->temperatureFn = NULL;
+    gyro->temperatureFn = icm42605ReadTemperature;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
     gyro->gyroAlign = gyro->busDev->param;