mirror of
https://github.com/sipeed/MaixCDK.git
synced 2026-09-12 22:59:44 -05:00
feat(lsm6dsowtr): refactor LSM6DSOWTR initialization and add I2C management
This commit is contained in:
taorye
@@ -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 <atomic>
|
||||
#include "maix_basic.hpp"
|
||||
#include <vector>
|
||||
#include <future>
|
||||
#include <cstdint>
|
||||
|
||||
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<float> read();
|
||||
|
||||
private:
|
||||
void* _data;
|
||||
imu::Mode __mode;
|
||||
bool __block;
|
||||
// gyro
|
||||
std::string __gyro_device_path;
|
||||
std::vector<imu::GyroScale> __anglvel_scale_available;
|
||||
std::vector<imu::GyroOdr> __anglvel_sampling_freq_available;
|
||||
imu::GyroScale __curr_anglvel_scale;
|
||||
imu::GyroOdr __curr_anglvel_sampling_freq;
|
||||
// accel
|
||||
std::string __accel_device_path;
|
||||
std::vector<imu::AccScale> __accel_scale_available;
|
||||
std::vector<imu::AccOdr> __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<std::pair<int, std::string>> open_future;
|
||||
bool open_fut_need_get = false;
|
||||
};
|
||||
}
|
||||
|
||||
} // namespace maix::ext_dev::lsm6dsowtr
|
||||
@@ -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 <iostream>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "maix_i2c.hpp"
|
||||
#include "maix_log.hpp"
|
||||
#include "maix_time.hpp"
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <map>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
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<int, I2cInfo> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::string> __get_device_paths() {
|
||||
std::vector<std::string> 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<uint8_t> read_reg_block(uint8_t reg, size_t len) {
|
||||
// 发送寄存器地址
|
||||
i2c->writeto(addr, std::vector<uint8_t>{reg});
|
||||
|
||||
// 读取 len 字节
|
||||
maix::Bytes* bytes = i2c->readfrom(addr, (int)len);
|
||||
std::vector<uint8_t> 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<uint8_t> 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<uint8_t>& b, size_t i) -> int16_t {
|
||||
return static_cast<int16_t>(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<int, std::string> {
|
||||
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<std::string> __string_to_vector_string(std::string input) {
|
||||
// std::istringstream iss(input);
|
||||
// std::vector<std::string> values;
|
||||
// std::string token;
|
||||
|
||||
// while (iss >> token) { // 按空格分割
|
||||
// values.push_back(token);
|
||||
// }
|
||||
// return values;
|
||||
// }
|
||||
|
||||
static std::vector<float> __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<Lsm6dsowI2C*>(this->_data);
|
||||
}
|
||||
|
||||
static std::vector<float> __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<float> LSM6DSOWTR::read() {
|
||||
auto dev = static_cast<Lsm6dsowI2C*>(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<float> LSM6DSOWTR::read()
|
||||
{
|
||||
std::vector<float> 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 {};
|
||||
}
|
||||
|
||||
}
|
||||
@@ -53,36 +53,21 @@ std::vector<ext_dev::imu::IMUInfo> 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<std::string>();
|
||||
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");
|
||||
|
||||
Reference in New Issue
Block a user