bumped version to 1.2

renamed LSM6DS3_ADDRESS to LSM6DS3_DEV_ADDRESS
moved LSM6DS3_DEG_TO_RAD into imu.h
refactored using sizeof
moved imu_t definition

application.fam

@@ -6,7 +6,7 @@ App(
     stack_size=10 * 1024,
     fap_category="GPIO",
     fap_icon="icon.png",
-    fap_version="1.1",
+    fap_version="1.2",
     fap_libs=["ble_profile"],
     sources=["*.c", "*.cc"],
     fap_private_libs=[

tracking/imu/imu_bmi160.c → tracking/imu/bmi160.c

@@ -1,7 +1,7 @@
-#include "imu_bmi160.h"
+#include "bmi160.h"
 
-static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
-static const double G = 9.81;
+#define BMI160_TAG "BMI160"
+#define BMI160_DEV_ADDR (0x69 << 1)
 
 struct bmi160_dev bmi160dev;
 struct bmi160_sensor_data bmi160_accel;
@@ -69,9 +69,9 @@ int bmi160_read(double* vec) {
         return 0;
     }
 
-    vec[0] = ((double)bmi160_accel.x * 4 / 32768) * G;
-    vec[1] = ((double)bmi160_accel.y * 4 / 32768) * G;
-    vec[2] = ((double)bmi160_accel.z * 4 / 32768) * G;
+    vec[0] = ((double)bmi160_accel.x * 4 / 32768) * GRAVITY;
+    vec[1] = ((double)bmi160_accel.y * 4 / 32768) * GRAVITY;
+    vec[2] = ((double)bmi160_accel.z * 4 / 32768) * GRAVITY;
     vec[3] = ((double)bmi160_gyro.x * 2000 / 32768) * DEG_TO_RAD;
     vec[4] = ((double)bmi160_gyro.y * 2000 / 32768) * DEG_TO_RAD;
     vec[5] = ((double)bmi160_gyro.z * 2000 / 32768) * DEG_TO_RAD;
@@ -81,3 +81,13 @@ int bmi160_read(double* vec) {
 
 void bmi160_end() {
 }
+
+struct imu_t imu_bmi160 = {
+   BMI160_DEV_ADDR,
+   bmi160_begin,
+   bmi160_end,
+   bmi160_read,
+   BMI160_TAG
+};
+
+

tracking/imu/imu_bmi160.h → tracking/imu/bmi160.h

@@ -5,10 +5,6 @@
 #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);
@@ -16,4 +12,6 @@ bool bmi160_begin(void);
 void bmi160_end(void);
 int bmi160_read(double* vec);
 
+extern struct imu_t imu_bmi160;
+
 #endif // IMU_BMI160_H

tracking/imu/imu.c

@@ -1,35 +1,21 @@
 #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"
-    }
+struct imu_t* imu_types[] = {
+    &imu_bmi160,
+    &imu_lsm6ds3trc,
+    &imu_lsm6dso
 };
 
-imu_t* imu_found;
+struct imu_t* imu_found;
 
 bool imu_begin() {
     furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
     if (imu_found == NULL) {
         imu_found = find_imu();
+        if (imu_found == NULL) {
+            furi_hal_i2c_release(&furi_hal_i2c_handle_external);
+            return false;
+        }
         FURI_LOG_E(IMU_TAG, "Found Device %s", imu_found->name);
     }
     bool ret = 0;
@@ -55,31 +41,13 @@ int imu_read(double* vec) {
     return ret;
 }
 
-imu_t* get_imu(uint8_t address) {
+struct imu_t* find_imu() {
     unsigned int i;
-    for(i = 0; i < 3; i++) {
-        if (imu_types[i].address == address) {
-            return &imu_types[i];
+    for(i = 0; i < sizeof(imu_types) / sizeof(struct imu_t*); i++) {
+        if(furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, imu_types[i]->address, 50)) {
+            FURI_LOG_E(IMU_TAG, "found i2c device address 0x%X", imu_types[i]->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;
-    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 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

@@ -4,9 +4,10 @@
 #include <stdbool.h>
 #include <furi_hal.h>
 
-#include "imu_bmi160.h"
-#include "imu_lsm6ds3trc.h"
-#include "imu_lsm6dso.h"
+#include "imu_t.h"
+#include "bmi160.h"
+#include "lsm6ds3trc.h"
+#include "lsm6dso.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -17,23 +18,17 @@ 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;
+static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
+static const double GRAVITY = 9.81;
 
 bool imu_begin();
 void imu_end();
 int imu_read(double* vec);
-imu_t* get_imu(uint8_t address);
+struct imu_t* get_imu(uint8_t address);
 unsigned int imu_scan_i2c();
-imu_t* find_imu();
+struct imu_t* find_imu();
 
 #ifdef __cplusplus
 }
 #endif
-#endif
+#endif // IMU_H

tracking/imu/imu_t.h

@@ -0,0 +1,13 @@
+#ifndef IMU_T_H   /* Include guard */
+#define IMU_T_H
+
+struct imu_t
+{
+    unsigned int address;
+    bool (*begin)(void);
+    void (*end)(void);
+    int (*read)(double* vec);
+    char* name;
+};
+
+#endif

tracking/imu/imu_lsm6ds3trc.c → tracking/imu/lsm6ds3trc.c

@@ -1,24 +1,25 @@
-#include "imu_lsm6ds3trc.h"
+#include "lsm6ds3trc.h"
 
-static const double LSM6DS3_DEG_TO_RAD = 0.017453292519943295769236907684886;
+#define LSM6DS3_TAG "LSM6DS3"
+#define LSM6DS3_DEV_ADDRESS (0x6A << 1)
 
 stmdev_ctx_t lsm6ds3trc_ctx;
 
 int32_t lsm6ds3trc_write_i2c(void* handle, uint8_t reg_addr, const uint8_t* data, uint16_t len) {
-    if(furi_hal_i2c_write_mem(handle, LSM6DS3_ADDRESS, reg_addr, (uint8_t*)data, len, 50))
+    if(furi_hal_i2c_write_mem(handle, LSM6DS3_DEV_ADDRESS, reg_addr, (uint8_t*)data, len, 50))
         return 0;
     return -1;
 }
 
 int32_t lsm6ds3trc_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len) {
-    if(furi_hal_i2c_read_mem(handle, LSM6DS3_ADDRESS, reg_addr, read_data, len, 50)) return 0;
+    if(furi_hal_i2c_read_mem(handle, LSM6DS3_DEV_ADDRESS, reg_addr, read_data, len, 50)) return 0;
     return -1;
 }
 
 bool lsm6ds3trc_begin() {
     FURI_LOG_I(LSM6DS3_TAG, "Init LSM6DS3TR-C");
 
-    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DS3_ADDRESS, 50)) {
+    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DS3_DEV_ADDRESS, 50)) {
         FURI_LOG_E(LSM6DS3_TAG, "Not ready");
         return false;
     }
@@ -76,11 +77,19 @@ 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]) * 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;
+        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;
         ret |= GYR_DATA_READY;
     }
 
     return ret;
 }
+
+struct imu_t imu_lsm6ds3trc = {
+    LSM6DS3_DEV_ADDRESS,
+    lsm6ds3trc_begin,
+    lsm6ds3trc_end,
+    lsm6ds3trc_read,
+    LSM6DS3_TAG
+};

tracking/imu/imu_lsm6ds3trc.h → tracking/imu/lsm6ds3trc.h

@@ -7,10 +7,6 @@
 
 #include "imu.h"
 
-#define LSM6DS3_TAG "LSM6DS3TR-C"
-
-#define LSM6DS3_ADDRESS (0x6A << 1)
-
 // External variable declaration
 extern stmdev_ctx_t lsm6ds3trc_ctx;
 
@@ -21,5 +17,6 @@ bool lsm6ds3trc_begin(void);
 void lsm6ds3trc_end(void);
 int lsm6ds3trc_read(double* vec);
 
+extern struct imu_t imu_lsm6ds3trc;
 
 #endif // LSM6DS3TRC_H

tracking/imu/imu_lsm6dso.c → tracking/imu/lsm6dso.c

@@ -1,4 +1,7 @@
-#include "imu_lsm6dso.h"
+#include "lsm6dso.h"
+
+#define LSM6DSO_TAG "LSM6DO"
+#define LSM6DSO_DEV_ADDRESS (0x6B << 1)
 
 stmdev_ctx_t lsm6dso_ctx;
 
@@ -76,11 +79,19 @@ int lsm6dso_read(double* vec) {
 
     if(reg.status_reg.gda) {
         lsm6dso_angular_rate_raw_get(&lsm6dso_ctx, data);
-        vec[5] = (double)lsm6dso_from_fs2000_to_mdps(data[0]) * LSM6DSO_DEG_TO_RAD / 1000;
-        vec[3] = (double)lsm6dso_from_fs2000_to_mdps(data[1]) * LSM6DSO_DEG_TO_RAD / 1000;
-        vec[4] = (double)lsm6dso_from_fs2000_to_mdps(data[2]) * LSM6DSO_DEG_TO_RAD / 1000;
+        vec[5] = (double)lsm6dso_from_fs2000_to_mdps(data[0]) * DEG_TO_RAD / 1000;
+        vec[3] = (double)lsm6dso_from_fs2000_to_mdps(data[1]) * DEG_TO_RAD / 1000;
+        vec[4] = (double)lsm6dso_from_fs2000_to_mdps(data[2]) * DEG_TO_RAD / 1000;
         ret |= GYR_DATA_READY;
     }
 
     return ret;
 }
+
+struct imu_t imu_lsm6dso = {
+    LSM6DSO_DEV_ADDRESS,
+    lsm6dso_begin,
+    lsm6dso_end,
+    lsm6dso_read,
+    LSM6DSO_TAG
+};

tracking/imu/imu_lsm6dso.h → tracking/imu/lsm6dso.h

@@ -1,4 +1,4 @@
-#ifndef IMU_LSM6DSO_H   /* Include guard */
+#ifndef IMU_LSM6DSO_H
 #define IMU_LSM6DSO_H
 
 #include "../../lib/lsm6dso-api/lsm6dso_reg.h"
@@ -7,12 +7,6 @@
 
 #include "imu.h"
 
-#define LSM6DSO_TAG "LSM6DO-C"
-
-#define LSM6DSO_DEV_ADDRESS UINT8_C(0xD6)
-
-static const double LSM6DSO_DEG_TO_RAD = 0.017453292519943295769236907684886;
-
 int32_t lsm6dso_write_i2c(void* handle, uint8_t reg_addr, uint8_t* data, uint16_t len);
 int32_t lsm6dso_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len);
 
@@ -20,4 +14,6 @@ bool lsm6dso_begin();
 void lsm6dso_end();
 int lsm6dso_read(double* vec);
 
-#endif
+extern struct imu_t imu_lsm6dso;
+
+#endif // IMU_LSM6DSO_H