From b7f561bfdde18f520ea672e1f93c207252a94f54 Mon Sep 17 00:00:00 2001
From: alex-vg <22611767+alex-vg@users.noreply.github.com>
Date: Wed, 29 May 2024 16:43:50 +0200
Subject: [PATCH] refactored imu headers

---
 tracking/imu/imu.c            | 17 ++++++++++-------
 tracking/imu/imu.h            |  8 +++++++-
 tracking/imu/imu_bmi160.c     | 28 ++++++++++------------------
 tracking/imu/imu_bmi160.h     | 18 ++++++++++++++++++
 tracking/imu/imu_lsm6ds3trc.c | 26 +++++++++-----------------
 tracking/imu/imu_lsm6ds3trc.h | 25 +++++++++++++++++++++++++
 tracking/imu/imu_lsm6dso.c    | 11 -----------
 tracking/imu/imu_lsm6dso.h    |  5 ++---
 8 files changed, 81 insertions(+), 57 deletions(-)
 create mode 100644 tracking/imu/imu_bmi160.h
 create mode 100644 tracking/imu/imu_lsm6ds3trc.h

diff --git a/tracking/imu/imu.c b/tracking/imu/imu.c
index b6be9f2e77a..b89a63c687f 100644
--- a/tracking/imu/imu.c
+++ b/tracking/imu/imu.c
@@ -1,12 +1,5 @@
 #include "imu.h"
 
-bool bmi160_begin();
-int bmi160_read(double* vec);
-
-bool lsm6ds3trc_begin();
-void lsm6ds3trc_end();
-int lsm6ds3trc_read(double* vec);
-
 bool imu_begin() {
     furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
     bool ret = lsm6dso_begin(); // lsm6ds3trc_begin();
@@ -26,3 +19,13 @@ int imu_read(double* vec) {
     furi_hal_i2c_release(&furi_hal_i2c_handle_external);
     return ret;
 }
+
+void imu_scan_i2c() {
+    unsigned int address;
+    unsigned int *found;
+    for(address = 1; address < 0xff; address++) {
+        if(furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, address, 50)) {
+            FURI_LOG_E(IMU_TAG, "<<<<<<<found Device>>>>>>> ID 0x%X", address);
+        }
+    }
+}
diff --git a/tracking/imu/imu.h b/tracking/imu/imu.h
index 220b302875d..bfd0e031d74 100644
--- a/tracking/imu/imu.h
+++ b/tracking/imu/imu.h
@@ -1,9 +1,12 @@
-#pragma once
+#ifndef IMU_H
+#define IMU_H
 
 #include <stdbool.h>
 
 #include <furi_hal.h>
 
+#include "imu_bmi160.h"
+#include "imu_lsm6ds3trc.h"
 #include "imu_lsm6dso.h"
 
 #ifdef __cplusplus
@@ -13,6 +16,8 @@ extern "C" {
 #define ACC_DATA_READY (1 << 0)
 #define GYR_DATA_READY (1 << 1)
 
+#define IMU_TAG "IMU_H"
+
 bool imu_begin();
 void imu_end();
 int imu_read(double* vec);
@@ -20,3 +25,4 @@ int imu_read(double* vec);
 #ifdef __cplusplus
 }
 #endif
+#endif
diff --git a/tracking/imu/imu_bmi160.c b/tracking/imu/imu_bmi160.c
index fbb461062ea..de3c6963ea3 100644
--- a/tracking/imu/imu_bmi160.c
+++ b/tracking/imu/imu_bmi160.c
@@ -1,12 +1,4 @@
-#include "../../lib/bmi160-api/bmi160.h"
-
-#include <furi_hal.h>
-
-#include "imu.h"
-
-#define TAG "BMI160"
-
-#define BMI160_DEV_ADDR (0x69 << 1)
+#include "imu_bmi160.h"
 
 static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
 static const double G = 9.81;
@@ -28,14 +20,14 @@ int8_t bmi160_read_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* read_data, u
 }
 
 bool bmi160_begin() {
-    FURI_LOG_I(TAG, "Init BMI160");
+    FURI_LOG_I(BMI160_TAG, "Init BMI160");
 
     if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, BMI160_DEV_ADDR, 50)) {
-        FURI_LOG_E(TAG, "Device not ready!");
+        FURI_LOG_E(BMI160_TAG, "Device not ready!");
         return false;
     }
 
-    FURI_LOG_I(TAG, "Device ready!");
+    FURI_LOG_I(BMI160_TAG, "Device ready!");
 
     bmi160dev.id = BMI160_DEV_ADDR;
     bmi160dev.intf = BMI160_I2C_INTF;
@@ -44,8 +36,8 @@ bool bmi160_begin() {
     bmi160dev.delay_ms = furi_delay_ms;
 
     if(bmi160_init(&bmi160dev) != BMI160_OK) {
-        FURI_LOG_E(TAG, "Initialization failure!");
-        FURI_LOG_E(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+        FURI_LOG_E(BMI160_TAG, "Initialization failure!");
+        FURI_LOG_E(BMI160_TAG, "Chip ID 0x%X", bmi160dev.chip_id);
         return false;
     }
 
@@ -59,13 +51,13 @@ bool bmi160_begin() {
     bmi160dev.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
 
     if(bmi160_set_sens_conf(&bmi160dev) != BMI160_OK) {
-        FURI_LOG_E(TAG, "Initialization failure!");
-        FURI_LOG_E(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+        FURI_LOG_E(BMI160_TAG, "Initialization failure!");
+        FURI_LOG_E(BMI160_TAG, "Chip ID 0x%X", bmi160dev.chip_id);
         return false;
     }
 
-    FURI_LOG_I(TAG, "Initialization success!");
-    FURI_LOG_I(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+    FURI_LOG_I(BMI160_TAG, "Initialization success!");
+    FURI_LOG_I(BMI160_TAG, "Chip ID 0x%X", bmi160dev.chip_id);
 
     return true;
 }
diff --git a/tracking/imu/imu_bmi160.h b/tracking/imu/imu_bmi160.h
new file mode 100644
index 00000000000..605a220fbc3
--- /dev/null
+++ b/tracking/imu/imu_bmi160.h
@@ -0,0 +1,18 @@
+#ifndef IMU_BMI160_H
+#define IMU_BMI160_H
+
+#include "../../lib/bmi160-api/bmi160.h"
+#include <furi_hal.h>
+#include "imu.h"
+
+// Define constants
+#define BMI160_TAG "BMI160"
+#define BMI160_DEV_ADDR (0x69 << 1)
+
+// Function declarations
+int8_t bmi160_write_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint16_t len);
+int8_t bmi160_read_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* read_data, uint16_t len);
+bool bmi160_begin(void);
+int bmi160_read(double* vec);
+
+#endif // IMU_BMI160_H
diff --git a/tracking/imu/imu_lsm6ds3trc.c b/tracking/imu/imu_lsm6ds3trc.c
index 9ae47e97acd..a7bfbf47e20 100644
--- a/tracking/imu/imu_lsm6ds3trc.c
+++ b/tracking/imu/imu_lsm6ds3trc.c
@@ -1,14 +1,6 @@
-#include "../../lib/lsm6ds3tr-api/lsm6ds3tr-c_reg.h"
+#include "imu_lsm6ds3trc.h"
 
-#include <furi_hal.h>
-
-#include "imu.h"
-
-#define TAG "LSM6DS3TR-C"
-
-#define LSM6DS3_ADDRESS (0x6A << 1)
-
-static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
+static const double LSM6DS3_DEG_TO_RAD = 0.017453292519943295769236907684886;
 
 stmdev_ctx_t lsm6ds3trc_ctx;
 
@@ -24,10 +16,10 @@ int32_t lsm6ds3trc_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data,
 }
 
 bool lsm6ds3trc_begin() {
-    FURI_LOG_I(TAG, "Init LSM6DS3TR-C");
+    FURI_LOG_I(LSM6DS3_TAG, "Init LSM6DS3TR-C");
 
     if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DS3_ADDRESS, 50)) {
-        FURI_LOG_E(TAG, "Not ready");
+        FURI_LOG_E(LSM6DS3_TAG, "Not ready");
         return false;
     }
 
@@ -39,7 +31,7 @@ bool lsm6ds3trc_begin() {
     uint8_t whoami;
     lsm6ds3tr_c_device_id_get(&lsm6ds3trc_ctx, &whoami);
     if(whoami != LSM6DS3TR_C_ID) {
-        FURI_LOG_I(TAG, "Unknown model: %x", (int)whoami);
+        FURI_LOG_I(LSM6DS3_TAG, "Unknown model: %x", (int)whoami);
         return false;
     }
 
@@ -59,7 +51,7 @@ bool lsm6ds3trc_begin() {
     lsm6ds3tr_c_gy_power_mode_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_HIGH_PERFORMANCE);
     lsm6ds3tr_c_gy_band_pass_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_LP2_ONLY);
 
-    FURI_LOG_I(TAG, "Init OK");
+    FURI_LOG_I(LSM6DS3_TAG, "Init OK");
     return true;
 }
 
@@ -84,9 +76,9 @@ int lsm6ds3trc_read(double* vec) {
 
     if(reg.status_reg.gda) {
         lsm6ds3tr_c_angular_rate_raw_get(&lsm6ds3trc_ctx, data);
-        vec[5] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[0]) * DEG_TO_RAD / 1000;
-        vec[3] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[1]) * DEG_TO_RAD / 1000;
-        vec[4] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[2]) * DEG_TO_RAD / 1000;
+        vec[5] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[0]) * LSM6DS3_DEG_TO_RAD / 1000;
+        vec[3] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[1]) * LSM6DS3_DEG_TO_RAD / 1000;
+        vec[4] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[2]) * LSM6DS3_DEG_TO_RAD / 1000;
         ret |= GYR_DATA_READY;
     }
 
diff --git a/tracking/imu/imu_lsm6ds3trc.h b/tracking/imu/imu_lsm6ds3trc.h
new file mode 100644
index 00000000000..c016d62db8e
--- /dev/null
+++ b/tracking/imu/imu_lsm6ds3trc.h
@@ -0,0 +1,25 @@
+#ifndef IMU_LSM6DS3TRC_H
+#define IMU_LSM6DS3TRC_H
+
+#include "../../lib/lsm6ds3tr-api/lsm6ds3tr-c_reg.h"
+
+#include <furi_hal.h>
+
+#include "imu.h"
+
+#define LSM6DS3_TAG "LSM6DS3TR-C"
+
+#define LSM6DS3_ADDRESS (0x6A << 1)
+
+// External variable declaration
+extern stmdev_ctx_t lsm6ds3trc_ctx;
+
+// Function declarations
+int32_t lsm6ds3trc_write_i2c(void* handle, uint8_t reg_addr, const uint8_t* data, uint16_t len);
+int32_t lsm6ds3trc_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len);
+bool lsm6ds3trc_begin(void);
+void lsm6ds3trc_end(void);
+int lsm6ds3trc_read(double* vec);
+
+
+#endif // LSM6DS3TRC_H
\ No newline at end of file
diff --git a/tracking/imu/imu_lsm6dso.c b/tracking/imu/imu_lsm6dso.c
index 9742929336c..0e70a5939c1 100644
--- a/tracking/imu/imu_lsm6dso.c
+++ b/tracking/imu/imu_lsm6dso.c
@@ -14,17 +14,6 @@ int32_t lsm6dso_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uin
     return -2;
 }
 
-void lsm6dso_scan_i2c() {
-    unsigned int address;
-    for(address = 1; address < 0xff; address++) {
-        if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, address, 50)) {
-                FURI_LOG_E(LSM6DSO_TAG, "No answer on ID 0x%X", address);
-        } else {
-            FURI_LOG_E(LSM6DSO_TAG, "<<<<<<<found Device>>>>>>> ID 0x%X", address);
-        }
-    }
-}
-
 bool lsm6dso_begin() {
     FURI_LOG_I(LSM6DSO_TAG, "Init LSM6DSOTR-C");
 
diff --git a/tracking/imu/imu_lsm6dso.h b/tracking/imu/imu_lsm6dso.h
index 77e24761707..c4cda5f3dea 100644
--- a/tracking/imu/imu_lsm6dso.h
+++ b/tracking/imu/imu_lsm6dso.h
@@ -1,5 +1,5 @@
-#ifndef LSM6DSO_H_   /* Include guard */
-#define LSM6DSO_H_
+#ifndef IMU_LSM6DSO_H   /* Include guard */
+#define IMU_LSM6DSO_H
 
 #include "../../lib/lsm6dso-api/lsm6dso_reg.h"
 
@@ -17,7 +17,6 @@ int32_t lsm6dso_write_i2c(void* handle, uint8_t reg_addr, uint8_t* data, uint16_
 int32_t lsm6dso_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len);
 
 bool lsm6dso_begin();
-void lsm6dso_scan_i2c();
 void lsm6dso_end();
 int lsm6dso_read(double* vec);