added lsm6dso version

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "lib/stm32-lsm6dso"]
+	path = lib/stm32-lsm6dso
+	url = https://github.com/STMicroelectronics/stm32-lsm6dso.git

application.fam

@@ -9,4 +9,10 @@ App(
     fap_version="1.1",
     fap_libs=["ble_profile"],
     sources=["*.c", "*.cc"],
+    fap_private_libs=[
+        Lib(
+            name="stm32-lsm6dso",
+            cflags=["-Wno-error"],
+        ),
+    ],
 )

build.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+ufbt

lib/stm32-lsm6dso

@@ -0,0 +1 @@
+Subproject commit 24788033c89e3148989e8d99d7e417edde0ae477

tracking/imu/imu.c

@@ -1,5 +1,4 @@
 #include "imu.h"
-#include <furi_hal.h>
 
 bool bmi160_begin();
 int bmi160_read(double* vec);
@@ -10,20 +9,20 @@ int lsm6ds3trc_read(double* vec);
 
 bool imu_begin() {
     furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
-    bool ret = bmi160_begin(); // lsm6ds3trc_begin();
+    bool ret = lsm6dso_begin(); // lsm6ds3trc_begin();
     furi_hal_i2c_release(&furi_hal_i2c_handle_external);
     return ret;
 }
 
 void imu_end() {
-    // furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
-    // lsm6ds3trc_end();
-    // furi_hal_i2c_release(&furi_hal_i2c_handle_external);
+    furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
+    lsm6dso_end();
+    furi_hal_i2c_release(&furi_hal_i2c_handle_external);
 }
 
 int imu_read(double* vec) {
     furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
-    int ret = bmi160_read(vec); // lsm6ds3trc_read(vec);
+    int ret = lsm6dso_read(vec); // lsm6ds3trc_read(vec);
     furi_hal_i2c_release(&furi_hal_i2c_handle_external);
     return ret;
 }

tracking/imu/imu.h

@@ -2,6 +2,10 @@
 
 #include <stdbool.h>
 
+#include <furi_hal.h>
+
+#include "imu_lsm6dso.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif

tracking/imu/imu_lsm6dso.c

@@ -0,0 +1,97 @@
+#include "imu_lsm6dso.h"
+
+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))
+        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))
+        return 0;
+    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");
+
+    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DSO_ADDRESS, 50)) {
+        FURI_LOG_E(LSM6DSO_TAG, "Not ready");
+        return false;
+    }
+
+    lsm6dso_ctx.write_reg = lsm6dso_write_i2c;
+    lsm6dso_ctx.read_reg = lsm6dso_read_i2c;
+    lsm6dso_ctx.mdelay = furi_delay_ms;
+    lsm6dso_ctx.handle = &furi_hal_i2c_handle_external;
+
+    uint8_t whoami;
+    lsm6dso_device_id_get(&lsm6dso_ctx, &whoami);
+    if(whoami != LSM6DSO_ID) {
+        FURI_LOG_I(LSM6DSO_TAG, "Unknown model: %x", (int)whoami);
+        return false;
+    }
+
+    lsm6dso_reset_set(&lsm6dso_ctx, PROPERTY_ENABLE);
+    uint8_t rst = PROPERTY_ENABLE;
+    while(rst) lsm6dso_reset_get(&lsm6dso_ctx, &rst);
+
+    lsm6dso_block_data_update_set(&lsm6dso_ctx, PROPERTY_ENABLE);
+    lsm6dso_fifo_mode_set(&lsm6dso_ctx, LSM6DSO_BYPASS_MODE);
+
+    lsm6dso_xl_data_rate_set(&lsm6dso_ctx, LSM6DSO_XL_ODR_104Hz);
+    lsm6dso_xl_full_scale_set(&lsm6dso_ctx, LSM6DSO_4g);
+    //lsm6dso_xl_lp1_bandwidth_set(&lsm6dso_ctx, LSM6DSO_XL_LP1_ODR_DIV_4);
+    //lsm6dso_aux_gy_lp1_bandwidth_set() ???
+
+    lsm6dso_gy_data_rate_set(&lsm6dso_ctx, LSM6DSO_GY_ODR_104Hz);
+    lsm6dso_gy_full_scale_set(&lsm6dso_ctx, LSM6DSO_2000dps);
+    lsm6dso_gy_power_mode_set(&lsm6dso_ctx, LSM6DSO_GY_HIGH_PERFORMANCE);
+    //lsm6dso_gy_band_pass_set(&lsm6dso_ctx, LSM6DSO_LP2_ONLY);
+
+    FURI_LOG_I(LSM6DSO_TAG, "Init OK");
+    return true;
+}
+
+void lsm6dso_end() {
+    lsm6dso_xl_data_rate_set(&lsm6dso_ctx, LSM6DSO_XL_ODR_OFF);
+    lsm6dso_gy_data_rate_set(&lsm6dso_ctx, LSM6DSO_GY_ODR_OFF);
+}
+
+int lsm6dso_read(double* vec) {
+    int ret = 0;
+    int16_t data[3];
+    lsm6dso_reg_t reg;
+    lsm6dso_status_reg_get(&lsm6dso_ctx, &reg.status_reg);
+
+    if(reg.status_reg.xlda) {
+        lsm6dso_acceleration_raw_get(&lsm6dso_ctx, data);
+        vec[2] = (double)lsm6dso_from_fs2_to_mg(data[0]) / 1000;
+        vec[0] = (double)lsm6dso_from_fs2_to_mg(data[1]) / 1000;
+        vec[1] = (double)lsm6dso_from_fs2_to_mg(data[2]) / 1000;
+        ret |= ACC_DATA_READY;
+    }
+
+    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;
+        ret |= GYR_DATA_READY;
+    }
+
+    return ret;
+}

tracking/imu/imu_lsm6dso.h

@@ -0,0 +1,24 @@
+#ifndef LSM6DSO_H_   /* Include guard */
+#define LSM6DSO_H_
+
+#include "../../lib/stm32-lsm6dso/lsm6dso_reg.h"
+
+#include <furi_hal.h>
+
+#include "imu.h"
+
+#define LSM6DSO_TAG "LSM6DO-C"
+
+#define LSM6DSO_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);
+
+bool lsm6dso_begin();
+void lsm6dso_scan_i2c();
+void lsm6dso_end();
+int lsm6dso_read(double* vec);
+
+#endif