diff --git a/components/ext_devs/ext_dev/include/maix_lsm6dsowtr.hpp b/components/ext_devs/ext_dev/include/maix_lsm6dsowtr.hpp index 23a634b9..84b64b79 100644 --- a/components/ext_devs/ext_dev/include/maix_lsm6dsowtr.hpp +++ b/components/ext_devs/ext_dev/include/maix_lsm6dsowtr.hpp @@ -1,74 +1,32 @@ -/** - * @author iawak9lkm - * @copyright Sipeed Ltd 2024- - * @license Apache 2.0 - * @update 2024.8.6: Add framework, create this file. - */ - #pragma once -#include "maix_basic.hpp" + #include "maix_imu.hpp" -#include +#include "maix_basic.hpp" +#include #include +#include namespace maix::ext_dev::lsm6dsowtr { -/** - * LSM6DSOWTR driver class - * @maixpy maix.ext_dev.lsm6dsowtr.LSM6DSOWTR - */ class LSM6DSOWTR { public: - /** - * @brief Construct a new LSM6DSOWTR object, will open LSM6DSOWTR - * - * @param mode LSM6DSOWTR Mode: ACC_ONLY/GYRO_ONLY/DUAL - * @param acc_scale acc scale, see @imu::AccScale - * @param acc_odr acc output data rate, see @imu::AccOdr - * @param gyro_scale gyro scale, see @imu::GyroScale - * @param gyro_odr gyro output data rate, see @imu::GyroOdr - * @param block block or non-block, defalut is true - * - * @maixpy maix.ext_dev.lsm6dsowtr.LSM6DSOWTR.__init__ - */ - LSM6DSOWTR( - maix::ext_dev::imu::Mode mode=maix::ext_dev::imu::Mode::DUAL, - maix::ext_dev::imu::AccScale acc_scale=maix::ext_dev::imu::AccScale::ACC_SCALE_2G, - maix::ext_dev::imu::AccOdr acc_odr=maix::ext_dev::imu::AccOdr::ACC_ODR_8000, - maix::ext_dev::imu::GyroScale gyro_scale=maix::ext_dev::imu::GyroScale::GYRO_SCALE_16DPS, - maix::ext_dev::imu::GyroOdr gyro_odr=maix::ext_dev::imu::GyroOdr::GYRO_ODR_8000, - bool block=true); + LSM6DSOWTR(int i2c_bus = -1, uint8_t addr = 0x6B, + imu::Mode mode = imu::Mode::DUAL, + imu::AccScale acc_scale = imu::AccScale::ACC_SCALE_2G, + imu::AccOdr acc_odr = imu::AccOdr::ACC_ODR_104, + imu::GyroScale gyro_scale = imu::GyroScale::GYRO_SCALE_250DPS, + imu::GyroOdr gyro_odr = imu::GyroOdr::GYRO_ODR_104, + bool block = false); + ~LSM6DSOWTR(); - LSM6DSOWTR(const LSM6DSOWTR&) = delete; - LSM6DSOWTR& operator=(const LSM6DSOWTR&) = delete; - LSM6DSOWTR(LSM6DSOWTR&&) = delete; - LSM6DSOWTR& operator=(LSM6DSOWTR&&) = delete; - - /** - * @brief Read data from LSM6DSOWTR. - * - * @return list type. If only one of the outputs is initialized, only [x,y,z] of that output will be returned. - * If all outputs are initialized, [acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z] is returned. - * - * @maixpy maix.ext_dev.lsm6dsowtr.LSM6DSOWTR.read - */ std::vector read(); + private: - void* _data; - imu::Mode __mode; - bool __block; - // gyro - std::string __gyro_device_path; - std::vector __anglvel_scale_available; - std::vector __anglvel_sampling_freq_available; - imu::GyroScale __curr_anglvel_scale; - imu::GyroOdr __curr_anglvel_sampling_freq; - // accel - std::string __accel_device_path; - std::vector __accel_scale_available; - std::vector __accel_sampling_freq_available; - imu::AccScale __curr_accel_scale; - imu::AccOdr __curr_accel_sampling_freq; + void* _data = nullptr; // points to priv::Lsm6dsowI2C + imu::Mode _mode; + std::future> open_future; + bool open_fut_need_get = false; }; -} \ No newline at end of file + +} // namespace maix::ext_dev::lsm6dsowtr \ No newline at end of file diff --git a/components/ext_devs/ext_dev/src/lsm6dsowtr/maix_lsm6dsowtr.cpp b/components/ext_devs/ext_dev/src/lsm6dsowtr/maix_lsm6dsowtr.cpp index 69baa3b5..f9f1a4ed 100644 --- a/components/ext_devs/ext_dev/src/lsm6dsowtr/maix_lsm6dsowtr.cpp +++ b/components/ext_devs/ext_dev/src/lsm6dsowtr/maix_lsm6dsowtr.cpp @@ -1,219 +1,340 @@ /** - * @author lxowalle - * @copyright Sipeed Ltd 2024- + * @author Your Name + * @copyright Sipeed Ltd 2025 * @license Apache 2.0 - * @update 2025-09-02: Add framework, create this file. + * @brief LSM6DSOW driver using Maix I2C abstraction (no ioctl) */ #include "maix_lsm6dsowtr.hpp" -#include -#include -#include -#include +#include "maix_i2c.hpp" +#include "maix_log.hpp" +#include "maix_time.hpp" +#include +#include +#include +#include +#include -namespace maix::ext_dev::lsm6dsowtr { -#define GYRO_DEVICE_NAME "lsm6dsox_gyro" -#define ACCEL_DEVICE_NAME "lsm6dsox_accel" +namespace maix::ext_dev::lsm6dsowtr::priv { -static std::string __load_file_to_string(std::string path) { - FILE* fp = fopen(path.c_str(), "r"); - if (!fp) { - perror("fopen"); - return ""; +// --- I2C bus manager (shared across devices) --- +static const int DEFAULT_I2C_BUS = 1; +static const char* TAG = "MAIX LSM6DSOW"; + +struct I2cInfo { + ::maix::peripheral::i2c::I2C* i2c_bus{nullptr}; + int dev_num{0}; +}; + +static std::recursive_mutex mtx; +static std::map i2c_manager; + +::maix::peripheral::i2c::I2C* get_i2c_bus(int bus, uint32_t freq_khz, bool& is_new) { + int _bus = (bus < 0) ? DEFAULT_I2C_BUS : bus; + std::lock_guard lock(mtx); + auto it = i2c_manager.find(_bus); + if (it != i2c_manager.end()) { + is_new = false; + it->second.dev_num++; + return it->second.i2c_bus; } - - std::string content; - char buffer[128]; - - while (fgets(buffer, sizeof(buffer), fp)) { - content += buffer; - } - - fclose(fp); - return content; + is_new = true; + auto i2c = new ::maix::peripheral::i2c::I2C(_bus, ::maix::peripheral::i2c::Mode::MASTER, (int)freq_khz * 1000); + i2c->scan(); // optional + I2cInfo info{.i2c_bus = i2c, .dev_num = 1}; + i2c_manager[_bus] = info; + return i2c; } -static bool __write_string_to_file(std::string path, std::string content) { - FILE* fp = fopen(path.c_str(), "w"); - if (!fp) { - perror("fopen"); - return false; - } - - fprintf(fp, "%s", content.c_str()); - fclose(fp); - return true; -} - -static float __invert_gyro_scale_to_float(imu::GyroScale scale) { - if (scale >= imu::GyroScale::GYRO_SCALE_2000DPS) { - return 0.001221729f; - } else if (scale >= imu::GyroScale::GYRO_SCALE_1000DPS) { - return 0.000610865f; - } else if (scale >= imu::GyroScale::GYRO_SCALE_500DPS) { - return 0.000305432f; - } else { - return 0.000152716f; +void release_i2c_bus(int bus) { + std::lock_guard lock(mtx); + auto it = i2c_manager.find(bus); + if (it == i2c_manager.end()) return; + it->second.dev_num--; + if (it->second.dev_num <= 0) { + delete it->second.i2c_bus; + i2c_manager.erase(it->first); } } -static float __invert_gyro_sampling_rate_to_float(imu::GyroOdr odr) { - if (odr <= imu::GyroOdr::GYRO_ODR_833) { - return 833; - } else if (odr <= imu::GyroOdr::GYRO_ODR_416) { - return 416; - } else if (odr <= imu::GyroOdr::GYRO_ODR_208) { - return 208; - } else if (odr <= imu::GyroOdr::GYRO_ODR_104) { - return 104; - } else if (odr <= imu::GyroOdr::GYRO_ODR_52) { - return 52; - } else if (odr <= imu::GyroOdr::GYRO_ODR_26) { - return 26; - } else { - return 12.5; - } -} +// --- LSM6DSOW register map --- +static constexpr uint8_t WHO_AM_I = 0x0F; +static constexpr uint8_t STATUS_REG = 0x1E; +static constexpr uint8_t OUTX_L_A = 0x28; +static constexpr uint8_t OUTX_L_G = 0x22; +static constexpr uint8_t CTRL1_XL = 0x10; +static constexpr uint8_t CTRL2_G = 0x11; -static float __invert_accel_scale_to_float(imu::AccScale scale) { - if (scale >= imu::AccScale::ACC_SCALE_16G) { - return 0.004785645f; - } else if (scale >= imu::AccScale::ACC_SCALE_8G) { - return 0.002392822f; - } else if (scale >= imu::AccScale::ACC_SCALE_4G) { - return 0.001196411f; - } else { - return 0.000598205f; - } -} +static constexpr float ACC_SENS_TABLE[4] = {0.061f, 0.122f, 0.244f, 0.488f}; // mg/LSB +static constexpr float GYRO_SENS_TABLE[4] = {8.75f, 17.5f, 35.0f, 70.0f}; // mdps/LSB +static constexpr float GRAVITY = 9.80665f; +static constexpr float ODR_TABLE[11] = { + 0, 12.5f, 26, 52, 104, 208, 416, 833, 1660, 3330, 6660 +}; -static float __invert_accel_sampling_rate_to_float(imu::AccOdr odr) { - if (odr <= imu::AccOdr::ACC_ODR_833) { - return 833; - } else if (odr <= imu::AccOdr::ACC_ODR_416) { - return 416; - } else if (odr <= imu::AccOdr::ACC_ODR_208) { - return 208; - } else if (odr <= imu::AccOdr::ACC_ODR_104) { - return 104; - } else if (odr <= imu::AccOdr::ACC_ODR_52) { - return 52; - } else if (odr <= imu::AccOdr::ACC_ODR_26) { - return 26; - } else { - return 12.5; - } -} +struct lsm6dsow_data_t { + struct { float x, y, z; } acc_xyz; + struct { float x, y, z; } gyro_xyz; + float temperature = 0.0f; +}; -static std::vector __get_device_paths() { - std::vector device_paths; - auto iio_device_base_path = "/sys/bus/iio/devices/iio:device"; - int idx = 0; - while (!app::need_exit()) { - auto path = iio_device_base_path + std::to_string(idx) + "/name"; - FILE *f = fopen(path.c_str(), "r"); - if (f) { - char name[256]; - if (fgets(name, sizeof(name), f) != NULL) { - name[strcspn(name, "\r\n")] = 0; - if (!strcmp(name, GYRO_DEVICE_NAME) || !strcmp(name, ACCEL_DEVICE_NAME)) { - device_paths.push_back(iio_device_base_path + std::to_string(idx)); - } +class Lsm6dsowI2C { +public: + ::maix::peripheral::i2c::I2C* i2c = nullptr; + uint8_t addr = 0x6B; + bool is_open = false; + uint8_t device_id = 0; + + Lsm6dsowI2C(int bus, uint8_t _addr, uint32_t freq_khz) + : addr(_addr) { + bool is_new; + i2c = get_i2c_bus(bus, freq_khz, is_new); + } + + ~Lsm6dsowI2C() { + // Note: i2c bus is managed globally, not deleted here + } + + uint8_t read_reg(uint8_t reg) { + return this->read_reg_block(reg, 1)[0]; + } + + std::vector read_reg_block(uint8_t reg, size_t len) { + // 发送寄存器地址 + i2c->writeto(addr, std::vector{reg}); + + // 读取 len 字节 + maix::Bytes* bytes = i2c->readfrom(addr, (int)len); + std::vector result; + if (bytes && bytes->size() >= len) { + result.assign(bytes->begin(), bytes->begin() + len); + } + delete bytes; + return result; + } + + void write_reg(uint8_t reg, uint8_t val) { + std::vector data = {reg, val}; // [寄存器地址, 值] + i2c->writeto(addr, data); + } + + bool check_device() { + device_id = read_reg(WHO_AM_I); + return (device_id == 0x6C); // LSM6DSOW WHO_AM_I = 0x6C + } + + void set_acc_odr(float hz) { + int best_idx = 0; + float min_diff = std::abs(ODR_TABLE[0] - hz); + for (int i = 1; i < 11; ++i) { + float diff = std::abs(ODR_TABLE[i] - hz); + if (diff < min_diff) { + min_diff = diff; + best_idx = i; } - fclose(f); - idx ++; - } else { - break; + } + uint8_t val = read_reg(CTRL1_XL); + val = (val & 0x0F) | (best_idx << 4); + write_reg(CTRL1_XL, val); + } + + void set_acc_scale(int g_val) { + int fs = 0; + if (g_val == 2) fs = 0; + else if (g_val == 4) fs = 1; + else if (g_val == 8) fs = 2; + else if (g_val == 16) fs = 3; + uint8_t val = read_reg(CTRL1_XL); + val = (val & ~0x0C) | (fs << 2); + write_reg(CTRL1_XL, val); + } + + void set_gyro_odr(float hz) { + int best_idx = 0; + float min_diff = std::abs(ODR_TABLE[0] - hz); + for (int i = 1; i < 11; ++i) { + float diff = std::abs(ODR_TABLE[i] - hz); + if (diff < min_diff) { + min_diff = diff; + best_idx = i; + } + } + uint8_t val = read_reg(CTRL2_G); + val = (val & 0x0F) | (best_idx << 4); + write_reg(CTRL2_G, val); + } + + void set_gyro_scale(int dps_val) { + int fs = 0; + if (dps_val == 250) fs = 0; + else if (dps_val == 500) fs = 1; + else if (dps_val == 1000) fs = 2; + else if (dps_val == 2000) fs = 3; + uint8_t val = read_reg(CTRL2_G); + val = (val & ~0x0C) | (fs << 2); + write_reg(CTRL2_G, val); + } + + bool read_data(lsm6dsow_data_t& out) { + uint8_t status = read_reg(STATUS_REG); + if ((status & 0x01) == 0 || (status & 0x02) == 0) { + return false; // no new data + } + + auto acc_bytes = read_reg_block(OUTX_L_A, 6); + auto gyro_bytes = read_reg_block(OUTX_L_G, 6); + + auto to_i16 = [](const std::vector& b, size_t i) -> int16_t { + return static_cast(b[i] | (b[i+1] << 8)); + }; + + int16_t acc_raw[3] = { + to_i16(acc_bytes, 0), + to_i16(acc_bytes, 2), + to_i16(acc_bytes, 4) + }; + int16_t gyro_raw[3] = { + to_i16(gyro_bytes, 0), + to_i16(gyro_bytes, 2), + to_i16(gyro_bytes, 4) + }; + + float acc_sens = ACC_SENS_TABLE[(read_reg(CTRL1_XL) >> 2) & 0x03]; + float gyro_sens = GYRO_SENS_TABLE[(read_reg(CTRL2_G) >> 2) & 0x03]; + + for (int i = 0; i < 3; ++i) { + out.acc_xyz.x = acc_raw[0] * acc_sens * 1e-3f * GRAVITY; + out.acc_xyz.y = acc_raw[1] * acc_sens * 1e-3f * GRAVITY; + out.acc_xyz.z = acc_raw[2] * acc_sens * 1e-3f * GRAVITY; + out.gyro_xyz.x = gyro_raw[0] * gyro_sens * 1e-3f; + out.gyro_xyz.y = gyro_raw[1] * gyro_sens * 1e-3f; + out.gyro_xyz.z = gyro_raw[2] * gyro_sens * 1e-3f; + } + return true; + } +}; + +} // namespace maix::ext_dev::lsm6dsowtr::priv + +// --- Public API --- + +using namespace maix::ext_dev::lsm6dsowtr::priv; +namespace maix::ext_dev::lsm6dsowtr { + +LSM6DSOWTR::LSM6DSOWTR(int i2c_bus, uint8_t addr, + imu::Mode mode, + imu::AccScale acc_scale, + imu::AccOdr acc_odr, + imu::GyroScale gyro_scale, + imu::GyroOdr gyro_odr, + bool block) + : _mode(mode) +{ + auto dev = new Lsm6dsowI2C(i2c_bus, addr, 400); // 400kHz + _data = dev; + + // Map enums to numeric values + int acc_g = 2; + switch (acc_scale) { + case imu::AccScale::ACC_SCALE_2G: acc_g = 2; break; + case imu::AccScale::ACC_SCALE_4G: acc_g = 4; break; + case imu::AccScale::ACC_SCALE_8G: acc_g = 8; break; + case imu::AccScale::ACC_SCALE_16G: acc_g = 16; break; + default: acc_g = 16; break; + } + + int gyro_dps = 250; + switch (gyro_scale) { + case imu::GyroScale::GYRO_SCALE_250DPS: gyro_dps = 250; break; + case imu::GyroScale::GYRO_SCALE_500DPS: gyro_dps = 500; break; + case imu::GyroScale::GYRO_SCALE_1000DPS: gyro_dps = 1000; break; + case imu::GyroScale::GYRO_SCALE_2000DPS: gyro_dps = 2000; break; + default: gyro_dps = 2000; break; + } + + float acc_hz = 104.0f; + switch (acc_odr) { + case imu::AccOdr::ACC_ODR_12_5: acc_hz = 12.5f; break; + case imu::AccOdr::ACC_ODR_26: acc_hz = 26; break; + case imu::AccOdr::ACC_ODR_52: acc_hz = 52; break; + case imu::AccOdr::ACC_ODR_104: acc_hz = 104; break; + case imu::AccOdr::ACC_ODR_208: acc_hz = 208; break; + case imu::AccOdr::ACC_ODR_416: acc_hz = 416; break; + case imu::AccOdr::ACC_ODR_833: acc_hz = 833; break; + default: acc_hz = 833; break; + } + + float gyro_hz = 104.0f; + switch (gyro_odr) { + case imu::GyroOdr::GYRO_ODR_12_5: gyro_hz = 12.5f; break; + case imu::GyroOdr::GYRO_ODR_26: gyro_hz = 26; break; + case imu::GyroOdr::GYRO_ODR_52: gyro_hz = 52; break; + case imu::GyroOdr::GYRO_ODR_104: gyro_hz = 104; break; + case imu::GyroOdr::GYRO_ODR_208: gyro_hz = 208; break; + case imu::GyroOdr::GYRO_ODR_416: gyro_hz = 416; break; + case imu::GyroOdr::GYRO_ODR_833: gyro_hz = 833; break; + default: gyro_hz = 833; break; + } + + auto init_task = [dev, acc_g, acc_hz, gyro_dps, gyro_hz]() -> std::pair { + if (!dev->check_device()) { + maix::log::error("[%s] Invalid WHO_AM_I: 0x%02x", TAG, dev->device_id); + return {-1, "Device not found"}; + } + + dev->set_acc_scale(acc_g); + dev->set_acc_odr(acc_hz); + dev->set_gyro_scale(gyro_dps); + dev->set_gyro_odr(gyro_hz); + dev->is_open = true; + + maix::log::info("[%s] Open IMU Succ. Device ID: 0x%02x", TAG, dev->device_id); + return {0, ""}; + }; + + this->open_future = std::async(std::launch::async, init_task); + this->open_fut_need_get = true; + + if (block) { + maix::log::info("[%s] Waiting for IMU open...", TAG); + auto ret = this->open_future.get(); + this->open_fut_need_get = false; + if (ret.first < 0) { + maix::log::error("[%s] Failed: %s", TAG, ret.second.c_str()); } } - - return device_paths; } -// static std::vector __string_to_vector_string(std::string input) { -// std::istringstream iss(input); -// std::vector values; -// std::string token; - -// while (iss >> token) { // 按空格分割 -// values.push_back(token); -// } -// return values; -// } - -static std::vector __read_gyro_data(std::string path) { - auto gyro_scale = (float)std::atof(__load_file_to_string(path + "/in_anglvel_scale").c_str()); - auto gyro_x = (float)std::atof(__load_file_to_string(path + "/in_anglvel_x_raw").c_str()); - auto gyro_y = (float)std::atof(__load_file_to_string(path + "/in_anglvel_y_raw").c_str()); - auto gyro_z = (float)std::atof(__load_file_to_string(path + "/in_anglvel_z_raw").c_str()); - return {gyro_x * gyro_scale * 180 / (float)M_PI, gyro_y * gyro_scale * 180 / (float)M_PI, gyro_z * gyro_scale * 180 / (float)M_PI}; +LSM6DSOWTR::~LSM6DSOWTR() { + if (this->open_fut_need_get) { + this->open_future.wait(); + this->open_fut_need_get = false; + } + delete static_cast(this->_data); } -static std::vector __read_accel_data(std::string path) { - auto accel_scale = (float)std::atof(__load_file_to_string(path + "/in_accel_scale").c_str()); - auto accel_x = (float)std::atof(__load_file_to_string(path + "/in_accel_x_raw").c_str()); - auto accel_y = (float)std::atof(__load_file_to_string(path + "/in_accel_y_raw").c_str()); - auto accel_z = (float)std::atof(__load_file_to_string(path + "/in_accel_z_raw").c_str()); - return {accel_x * accel_scale / 9.8f, accel_y * accel_scale / 9.8f, accel_z * accel_scale / 9.8f}; -} - -LSM6DSOWTR::LSM6DSOWTR(imu::Mode mode, imu::AccScale acc_scale, - imu::AccOdr acc_odr, imu::GyroScale gyro_scale, imu::GyroOdr gyro_odr, bool block) -{ - auto device_paths = __get_device_paths(); - bool found_gyro_device = false, found_accel_device = false; - for (auto path : device_paths) { - auto name = __load_file_to_string(path + "/name"); - name[strcspn(name.c_str(), "\r\n")] = 0; - if (!strcmp(name.c_str(), GYRO_DEVICE_NAME)) { - found_gyro_device = true; - __gyro_device_path = path; - } else if (!strcmp(name.c_str(), ACCEL_DEVICE_NAME)) { - found_accel_device = true; - __accel_device_path = path; - } +std::vector LSM6DSOWTR::read() { + auto dev = static_cast(this->_data); + if (!dev->is_open) { + return {}; } - err::check_bool_raise(found_gyro_device, "Can't find gyro device!"); - err::check_bool_raise(found_accel_device, "Can't find accel device!"); - - __mode = mode; - __block = block; - - auto curr_acc_scale = __invert_accel_scale_to_float(acc_scale); - err::check_bool_raise(__write_string_to_file(__accel_device_path + "/in_accel_scale", std::to_string(curr_acc_scale)), "Config accel scale failed!"); - - auto curr_acc_odr = __invert_accel_sampling_rate_to_float(acc_odr); - err::check_bool_raise(__write_string_to_file(__accel_device_path + "/sampling_frequency", std::to_string(curr_acc_odr)), "Config accel odr failed!"); - - auto curr_gyro_scale = __invert_gyro_scale_to_float(gyro_scale); - err::check_bool_raise(__write_string_to_file(__gyro_device_path + "/in_anglvel_scale", std::to_string(curr_gyro_scale)), "Config gyro scale failed!"); - - auto curr_gyro_odr = __invert_gyro_sampling_rate_to_float(gyro_odr); - err::check_bool_raise(__write_string_to_file(__gyro_device_path + "/sampling_frequency", std::to_string(curr_gyro_odr)), "Config gyro odr failed!"); -} - -LSM6DSOWTR::~LSM6DSOWTR() -{ -} - -std::vector LSM6DSOWTR::read() -{ - std::vector ret; - if (__mode == imu::Mode::GYRO_ONLY) { - ret = __read_gyro_data(__gyro_device_path); - ret.push_back(0); - } else if (__mode == imu::Mode::ACC_ONLY) { - ret = __read_accel_data(__accel_device_path); - ret.push_back(0); - } else { - ret = __read_accel_data(__accel_device_path); - auto gyro_data = __read_gyro_data(__gyro_device_path); - ret.insert(ret.end(), gyro_data.begin(), gyro_data.end()); - ret.push_back(0); + lsm6dsow_data_t data; + if (!dev->read_data(data)) { + // No new data, return last or empty? Here we return zeros with temp=0 + // In real use, you may cache last valid frame + data = {}; } - return ret; + if (_mode == imu::Mode::DUAL) { + return {data.acc_xyz.x, data.acc_xyz.y, data.acc_xyz.z, + data.gyro_xyz.x, data.gyro_xyz.y, data.gyro_xyz.z, + data.temperature}; + } else if (_mode == imu::Mode::ACC_ONLY) { + return {data.acc_xyz.x, data.acc_xyz.y, data.acc_xyz.z, data.temperature}; + } else if (_mode == imu::Mode::GYRO_ONLY) { + return {data.gyro_xyz.x, data.gyro_xyz.y, data.gyro_xyz.z, data.temperature}; + } + return {}; } - } \ No newline at end of file diff --git a/components/ext_devs/ext_dev/src/maix_imu.cpp b/components/ext_devs/ext_dev/src/maix_imu.cpp index d48452bf..b355c6bf 100644 --- a/components/ext_devs/ext_dev/src/maix_imu.cpp +++ b/components/ext_devs/ext_dev/src/maix_imu.cpp @@ -53,36 +53,21 @@ std::vector get_imu_info() } #elif PLATFORM_MAIXCAM2 int default_i2c = 1; - auto iio_device_base_path = "/sys/bus/iio/devices/iio:device"; - auto collect_iio_device_names = std::vector(); - int idx = 0; - while (!app::need_exit()) { - auto path = iio_device_base_path + std::to_string(idx) + "/name"; - FILE *f = fopen(path.c_str(), "r"); - if (f) { - char name[256]; - if (fgets(name, sizeof(name), f) != NULL) { - name[strcspn(name, "\r\n")] = 0; - collect_iio_device_names.push_back(name); - } - fclose(f); - idx ++; - } else { - break; + auto bus = peripheral::i2c::I2C(default_i2c, peripheral::i2c::Mode::MASTER, 400000, peripheral::i2c::AddrSize::SEVEN_BIT); + auto res = bus.scan(); + for(auto r : res) + { + if(r == 0x6B) // QMI8658 default i2c addr + { + ext_dev::imu::IMUInfo lsm6dsow_info; + lsm6dsow_info.name = "LSM6DSOWTR"; + lsm6dsow_info.driver = "lsm6dsowtr"; + lsm6dsow_info.i2c_bus = default_i2c; + lsm6dsow_info.addr = 0x6B; + lsm6dsow_info.have_mag = false; // QMI8658 does not have mag + info.push_back(lsm6dsow_info); } } - - if ((std::find(collect_iio_device_names.begin(), collect_iio_device_names.end(), "lsm6dsox_gyro") != collect_iio_device_names.end())) { - ext_dev::imu::IMUInfo imu_info; - imu_info.name = "LSM6DSOWTR"; - imu_info.driver = "lsm6dsowtr"; - imu_info.i2c_bus = default_i2c; - imu_info.addr = 0x6B; - imu_info.have_mag = false; - info.push_back(imu_info); - } else { - return info; - } #endif return info; } @@ -139,7 +124,7 @@ IMU::IMU(std::string driver, int i2c_bus, int addr, int freq, imu::Mode mode, im param->driver.qmi8658 = new maix::ext_dev::qmi8658::QMI8658(i2c_bus, addr, freq, mode, acc_scale, acc_odr, gyro_scale, gyro_odr, block); break; case driver_type::lsm6dsowtr: - param->driver.lsm6dsowtr = new maix::ext_dev::lsm6dsowtr::LSM6DSOWTR(mode, acc_scale, acc_odr, gyro_scale, gyro_odr, block); + param->driver.lsm6dsowtr = new maix::ext_dev::lsm6dsowtr::LSM6DSOWTR(i2c_bus, addr, mode, acc_scale, acc_odr, gyro_scale, gyro_odr, block); break; default: throw err::Exception("Unsupported driver type");