From f331f3d47d16d56117f9fd0ff5b69120f2fc664d Mon Sep 17 00:00:00 2001
From: William Clark <wrvc96@gmail.com>
Date: Wed, 10 Jan 2024 15:39:14 +0000
Subject: [PATCH] temperature

---
 main.c    | 20 +++++++++++++++-----
 mpu6050.c | 33 ++++++++++++++++++++++-----------
 mpu6050.h | 10 ++++++++--
 3 files changed, 45 insertions(+), 18 deletions(-)

diff --git a/main.c b/main.c
index dc1c595..4ee7373 100644
--- a/main.c
+++ b/main.c
@@ -21,7 +21,7 @@ int main() {
         exit(1);
     }
 
-    mpu6050.cfg.gyro = MPU6050_GYRO_FS_2000;
+    mpu6050.cfg.gyro = MPU6050_GYRO_FS_250;
     mpu6050.cfg.acc = MPU6050_ACC_FS_2G;
 
     /* reset all signal paths */
@@ -71,11 +71,21 @@ LOOP:
         exit(1);
     }
 
-    printf("[GYRO °/s] x:%1.d  y:%1.d  z:%1.d ",
-        mpu6050.gyro.x, mpu6050.gyro.y, mpu6050.gyro.z);
+    if (mpu6050_read_temp(&mpu6050)) {
+        exit(1);
+    }
 
-    printf("[ACC mg] x: %-3d y: %-3d z: %-3d\n",
-        mpu6050.acc.x, mpu6050.acc.y, mpu6050.acc.z);
+    printf("[GYRO °/s] x:%4.1f  y:%4.1f  z:%4.1f ",
+        (float)mpu6050.data.gyro.x / 10.f,
+        (float)mpu6050.data.gyro.y / 10.f,
+        (float)mpu6050.data.gyro.z / 10.f);
+
+    printf("[ACC g] x:%4.3f  y:%4.3f  z:%4.3f ",
+        (float)mpu6050.data.acc.x / 1000.f,
+        (float)mpu6050.data.acc.y / 1000.f,
+        (float)mpu6050.data.acc.z / 1000.f);
+
+    printf("t:%4.1f°C\n", (float)mpu6050.data.temp / 10.f);
 
     usleep(100 * 1000);
     goto LOOP;
diff --git a/mpu6050.c b/mpu6050.c
index 85d0e91..cca6453 100644
--- a/mpu6050.c
+++ b/mpu6050.c
@@ -25,8 +25,7 @@ int mpu6050_init(mpu6050_t *mpu6050) {
     }
     
     memset(&mpu6050->cfg, 0, sizeof mpu6050->cfg);
-    memset(&mpu6050->acc, 0, sizeof mpu6050->acc);
-    memset(&mpu6050->gyro, 0, sizeof mpu6050->gyro);
+    memset(&mpu6050->data, 0, sizeof mpu6050->data);
 
     return err;
 }
@@ -41,7 +40,7 @@ int mpu6050_deinit(mpu6050_t *mpu6050) {
     return mpu6050->dev.deinit();
 }
 
-/* transform i16 samples st 1 lsb is 1 mg (1/1000 g) */
+/* transform accel i16 samples st 1 lsb is 1 mg (1/1000 g) */
 int mpu6050_read_acc(mpu6050_t *mpu6050) {
     uint8_t data[6];
     uint8_t shift;
@@ -52,14 +51,14 @@ int mpu6050_read_acc(mpu6050_t *mpu6050) {
     shift = acc_i16_shift(mpu6050->cfg.acc);
 
     err |= mpu6050->dev.read(REG_ACCEL_XOUT_H, data, 6);
-    mpu6050->acc.x = (int16_t)(data[0] << 8 | data[1]) >> shift;
-    mpu6050->acc.y = (int16_t)(data[2] << 8 | data[3]) >> shift;
-    mpu6050->acc.z = (int16_t)(data[4] << 8 | data[5]) >> shift;
+    mpu6050->data.acc.x = (int16_t)(data[0] << 8 | data[1]) >> shift;
+    mpu6050->data.acc.y = (int16_t)(data[2] << 8 | data[3]) >> shift;
+    mpu6050->data.acc.z = (int16_t)(data[4] << 8 | data[5]) >> shift;
 
     return err;
 }
 
-/* transform gyro i16 sample st 1 lsb = 0.1 deg / s */
+/* transform gyro i16 samples st 1 lsb = 0.1 deg / s */
 int mpu6050_read_gyro(mpu6050_t *mpu6050) {
     uint8_t data[6];
     uint8_t shift;
@@ -71,13 +70,25 @@ int mpu6050_read_gyro(mpu6050_t *mpu6050) {
 
     err |= mpu6050->dev.read(REG_GYRO_XOUT_H, data, 6);
 
-    mpu6050->gyro.x = ((int16_t)(data[0] << 8 | data[1]) >> shift);
-    mpu6050->gyro.y = ((int16_t)(data[2] << 8 | data[3]) >> shift);
-    mpu6050->gyro.z = ((int16_t)(data[4] << 8 | data[5]) >> shift);
+    mpu6050->data.gyro.x = ((int16_t)(data[0] << 8 | data[1]) >> shift);
+    mpu6050->data.gyro.y = ((int16_t)(data[2] << 8 | data[3]) >> shift);
+    mpu6050->data.gyro.z = ((int16_t)(data[4] << 8 | data[5]) >> shift);
 
     return err;
 }
 
+/* transform temp i16 sample st 1 lsb = 0.1 deg C */
+int mpu6050_read_temp(mpu6050_t *mpu6050) {
+    uint8_t data[2];
+    int err = 0;
+
+    assert(mpu6050);
+
+    err |= mpu6050->dev.read(REG_TEMP_OUT_H, data, 2);
+    mpu6050->data.temp = (int16_t)(data[0] << 8 | data[1])/34 + 365;
+    return err;
+}
+
 /* How much to shift down an i16 accel sample depending on full-scale mode set */
 static uint8_t acc_i16_shift(uint8_t fs) {
     switch(fs) {
@@ -89,7 +100,7 @@ static uint8_t acc_i16_shift(uint8_t fs) {
     }
 }
 
-/* How much to shift down an i16 sample depending on full-scale mode set */
+/* How much to shift down an i16 gyro sample depending on full-scale mode set */
 static uint8_t gyro_i16_shift(uint8_t fs) {
     switch(fs) {
         case MPU6050_GYRO_FS_250:  return 4;
diff --git a/mpu6050.h b/mpu6050.h
index 7d193bd..92bb124 100644
--- a/mpu6050.h
+++ b/mpu6050.h
@@ -42,11 +42,16 @@ struct mpu6050_gyroscope {
     int16_t z;
 };
 
+struct mpu6050_data {
+    struct mpu6050_accelerometer acc;
+    struct mpu6050_gyroscope gyro;
+    int16_t temp;
+};
+
 struct mpu6050 {
     struct mpu6050_dev dev;
     struct mpu6050_config cfg;
-    struct mpu6050_accelerometer acc;
-    struct mpu6050_gyroscope gyro;
+    struct mpu6050_data data;
 };
 
 typedef struct mpu6050 mpu6050_t;
@@ -55,5 +60,6 @@ int mpu6050_init(mpu6050_t *mpu6050);
 int mpu6050_deinit(mpu6050_t *mpu6050);
 int mpu6050_read_acc(mpu6050_t *mpu6050);
 int mpu6050_read_gyro(mpu6050_t *mpu6050);
+int mpu6050_read_temp(mpu6050_t *mpu6050);
 
 #endif
\ No newline at end of file