added dynamic lib usage depending on i2c address

tracking/imu/imu.c

@@ -1,31 +1,85 @@
 #include "imu.h"
 
+imu_t imu_types[] = {
+    {
+        BMI160_DEV_ADDR,
+        bmi160_begin,
+        bmi160_end,
+        bmi160_read,
+        "BMI160"
+    },
+    {
+        LSM6DS3_ADDRESS,
+        lsm6ds3trc_begin,
+        lsm6ds3trc_end,
+        lsm6ds3trc_read,
+        "LSM6DS3"
+    },
+    {
+        LSM6DSO_DEV_ADDRESS,
+        lsm6dso_begin,
+        lsm6dso_end,
+        lsm6dso_read,
+        "LSM6DSO"
+    }
+};
+
+imu_t* imu_found;
+
 bool imu_begin() {
     furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
-    bool ret = lsm6dso_begin(); // lsm6ds3trc_begin();
+    if (imu_found == NULL) {
+        imu_found = find_imu();
+        FURI_LOG_E(IMU_TAG, "Found Device %s", imu_found->name);
+    }
+    bool ret = 0;
+    if (imu_found != NULL) {
+        ret = imu_found->begin();
+    }
     furi_hal_i2c_release(&furi_hal_i2c_handle_external);
     return ret;
 }
 
 void imu_end() {
+    if (imu_found == NULL) return;
     furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
-    lsm6dso_end();
+    imu_found->end();
     furi_hal_i2c_release(&furi_hal_i2c_handle_external);
 }
 
 int imu_read(double* vec) {
+    if (imu_found == NULL) return 0;
     furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
-    int ret = lsm6dso_read(vec); // lsm6ds3trc_read(vec);
+    int ret = imu_found->read(vec);
     furi_hal_i2c_release(&furi_hal_i2c_handle_external);
     return ret;
 }
 
-void imu_scan_i2c() {
+imu_t* get_imu(uint8_t address) {
+    unsigned int i;
+    for(i = 0; i < 3; i++) {
+        if (imu_types[i].address == address) {
+            return &imu_types[i];
+        }
+    }
+    return NULL;
+}
+
+/**
+ * Gives the first found i2c address, there should be only one device connected.
+**/
+unsigned int 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);
+            FURI_LOG_E(IMU_TAG, "found i2c device address 0x%X", address);
+            return address;
         }
     }
+    return 0;
 }
+
+imu_t* find_imu() {
+   unsigned int address = imu_scan_i2c();
+   return get_imu(address);
+}

tracking/imu/imu.h

@@ -2,7 +2,6 @@
 #define IMU_H
 
 #include <stdbool.h>
-
 #include <furi_hal.h>
 
 #include "imu_bmi160.h"
@@ -18,9 +17,21 @@ extern "C" {
 
 #define IMU_TAG "IMU_H"
 
+typedef struct
+{
+    unsigned int address;
+    bool (*begin)(void);
+    void (*end)(void);
+    int (*read)(double* vec);
+    char name[255];
+} imu_t;
+
 bool imu_begin();
 void imu_end();
 int imu_read(double* vec);
+imu_t* get_imu(uint8_t address);
+unsigned int imu_scan_i2c();
+imu_t* find_imu();
 
 #ifdef __cplusplus
 }

tracking/imu/imu_bmi160.c

@@ -78,3 +78,6 @@ int bmi160_read(double* vec) {
 
     return ACC_DATA_READY | GYR_DATA_READY;
 }
+
+void bmi160_end() {
+}

tracking/imu/imu_bmi160.h

@@ -13,6 +13,7 @@
 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);
+void bmi160_end(void);
 int bmi160_read(double* vec);
 
 #endif // IMU_BMI160_H

tracking/imu/imu_lsm6dso.c

@@ -3,13 +3,13 @@
 stmdev_ctx_t lsm6dso_ctx;
 
 int32_t lsm6dso_write_i2c(void* handle, uint8_t reg_addr, uint8_t* data, uint16_t len) {
-    if(furi_hal_i2c_write_mem(handle, LSM6DSO_ADDRESS, reg_addr, data, len, 50))
+    if(furi_hal_i2c_write_mem(handle, LSM6DSO_DEV_ADDRESS, reg_addr, data, len, 50))
         return 0;
     return -2;
 }
 
 int32_t lsm6dso_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len) {
-    if(furi_hal_i2c_read_mem(handle, LSM6DSO_ADDRESS, reg_addr, read_data, len, 50))
+    if(furi_hal_i2c_read_mem(handle, LSM6DSO_DEV_ADDRESS, reg_addr, read_data, len, 50))
         return 0;
     return -2;
 }
@@ -17,7 +17,7 @@ int32_t lsm6dso_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uin
 bool lsm6dso_begin() {
     FURI_LOG_I(LSM6DSO_TAG, "Init LSM6DSOTR-C");
 
-    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DSO_ADDRESS, 50)) {
+    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DSO_DEV_ADDRESS, 50)) {
         FURI_LOG_E(LSM6DSO_TAG, "Not ready");
         return false;
     }

tracking/imu/imu_lsm6dso.h

@@ -9,7 +9,7 @@
 
 #define LSM6DSO_TAG "LSM6DO-C"
 
-#define LSM6DSO_ADDRESS UINT8_C(0xD6)
+#define LSM6DSO_DEV_ADDRESS UINT8_C(0xD6)
 
 static const double LSM6DSO_DEG_TO_RAD = 0.017453292519943295769236907684886;