Momentum-Apps/tracking/imu/imu.c

#include "imu.h"

#define IMU_TAG "IMU_H"

extern struct imu_t imu_bmi160;
extern struct imu_t imu_lsm6ds3trc;
extern struct imu_t imu_lsm6dso;

struct imu_t* imu_types[] = {
    &imu_bmi160,
    &imu_lsm6ds3trc,
    &imu_lsm6dso
};

static const int imu_count = sizeof(imu_types) / sizeof(struct imu_t*);

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();
    }

    bool ret = false;
    if (imu_found != NULL) {
        FURI_LOG_E(IMU_TAG, "Found Device %s", imu_found->name);
        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);
    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 = imu_found->read(vec);
    furi_hal_i2c_release(&furi_hal_i2c_handle_external);
    return ret;
}

struct imu_t* find_imu() {
    unsigned int i;
    for(i = 0; i < imu_count; 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;
}