Administrator/NANOKVM-MaixCDK-MIRROR
1
0
Fork 1
mirror of https://github.com/sipeed/MaixCDK.git synced 2026-11-09 17:04:38 -06:00
Code Issues Packages Projects Releases 1 Wiki Activity

* [maixcam2] support lsm6dsowtr

Browse Source
This commit is contained in:
lxowalle
2025-09-02 18:46:45 +08:00
parent 36be3fbf34
commit a008f2a18d
7 changed files with 373 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
components/ext_devs/ext_dev/CMakeLists.txt
View File

@@ -8,6 +8,7 @@ list(APPEND ADD_PRIVATE_INCLUDE "src/tmc2209")
list(APPEND ADD_PRIVATE_INCLUDE "src/tmc2209/janelia-arduino_TMC2209")
list(APPEND ADD_PRIVATE_INCLUDE "src/axp2101")
list(APPEND ADD_PRIVATE_INCLUDE "src/cmap")
list(APPEND ADD_PRIVATE_INCLUDE "src/lsm6dsowtr")
append_srcs_dir(ADD_SRCS "src")
append_srcs_dir(ADD_SRCS "src/bm8563")
@@ -19,6 +20,7 @@ append_srcs_dir(ADD_SRCS "src/tmc2209/janelia-arduino_TMC2209")
append_srcs_dir(ADD_SRCS "src/axp2101")
append_srcs_dir(ADD_SRCS "src/fp5510")
append_srcs_dir(ADD_SRCS "src/cmap")
append_srcs_dir(ADD_SRCS "src/lsm6dsowtr")
list(APPEND ADD_REQUIREMENTS basic peripheral json ahrs)

21
components/ext_devs/ext_dev/include/maix_imu.hpp
View File

@@ -43,13 +43,20 @@ enum class AccOdr {
ACC_ODR_4000, // Accelerometer ODR set to 4000 Hz.
ACC_ODR_2000, // Accelerometer ODR set to 2000 Hz.
ACC_ODR_1000, // Accelerometer ODR set to 1000 Hz.
ACC_ODR_833, // Accelerometer ODR set to 833 Hz.
ACC_ODR_500, // Accelerometer ODR set to 500 Hz.
ACC_ODR_416, // Accelerometer ODR set to 416 Hz.
ACC_ODR_250, // Accelerometer ODR set to 250 Hz.
ACC_ODR_208, // Accelerometer ODR set to 208 Hz.
ACC_ODR_128, // Accelerometer ODR set to 128 Hz.
ACC_ODR_125, // Accelerometer ODR set to 125 Hz.
ACC_ODR_104, // Accelerometer ODR set to 104 Hz.
ACC_ODR_62_5, // Accelerometer ODR set to 62.5 Hz.
ACC_ODR_52, // Accelerometer ODR set to 52 Hz.
ACC_ODR_31_25, // Accelerometer ODR set to 31.25 Hz.
ACC_ODR_128 = 12, // Accelerometer ODR set to 128 Hz.
ACC_ODR_26, // Accelerometer ODR set to 26 Hz.
ACC_ODR_21, // Accelerometer ODR set to 21 Hz.
ACC_ODR_12_5, // Accelerometer ODR set to 12.5 Hz.
ACC_ODR_11, // Accelerometer ODR set to 11 Hz.
ACC_ODR_3, // Accelerometer ODR set to 3 Hz.
};
@@ -62,10 +69,15 @@ enum class GyroScale {
GYRO_SCALE_16DPS = 0, // Gyroscope scale set to ±16 degrees per second.
GYRO_SCALE_32DPS, // Gyroscope scale set to ±32 degrees per second.
GYRO_SCALE_64DPS, // Gyroscope scale set to ±64 degrees per second.
GYRO_SCALE_125DPS, // Gyroscope scale set to ±125 degrees per second.
GYRO_SCALE_128DPS, // Gyroscope scale set to ±128 degrees per second.
GYRO_SCALE_250DPS, // Gyroscope scale set to ±250 degrees per second.
GYRO_SCALE_256DPS, // Gyroscope scale set to ±256 degrees per second.
GYRO_SCALE_500DPS, // Gyroscope scale set to ±500 degrees per second.
GYRO_SCALE_512DPS, // Gyroscope scale set to ±512 degrees per second.
GYRO_SCALE_1000DPS, // Gyroscope scale set to ±1000 degrees per second.
GYRO_SCALE_1024DPS, // Gyroscope scale set to ±1024 degrees per second.
GYRO_SCALE_2000DPS, // Gyroscope scale set to ±2000 degrees per second.
GYRO_SCALE_2048DPS, // Gyroscope scale set to ±2048 degrees per second.
};
@@ -78,11 +90,18 @@ enum class GyroOdr {
GYRO_ODR_4000, // Gyroscope ODR set to 4000 Hz.
GYRO_ODR_2000, // Gyroscope ODR set to 2000 Hz.
GYRO_ODR_1000, // Gyroscope ODR set to 1000 Hz.
GYRO_ODR_833, // Gyroscope ODR set to 833 Hz.
GYRO_ODR_500, // Gyroscope ODR set to 500 Hz.
GYRO_ODR_416, // Gyroscope ODR set to 416 Hz.
GYRO_ODR_250, // Gyroscope ODR set to 250 Hz.
GYRO_ODR_125, // Gyroscope ODR set to 125 Hz.
GYRO_ODR_208, // Gyroscope ODR set to 208 Hz.
GYRO_ODR_104, // Gyroscope ODR set to 104 Hz.
GYRO_ODR_62_5, // Gyroscope ODR set to 62.5 Hz.
GYRO_ODR_52, // Gyroscope ODR set to 52 Hz.
GYRO_ODR_26, // Gyroscope ODR set to 26 Hz.
GYRO_ODR_31_25, // Gyroscope ODR set to 31.25 Hz.
GYRO_ODR_12_5, // Gyroscope ODR set to 12.5 Hz.
};
/**

74
components/ext_devs/ext_dev/include/maix_lsm6dsowtr.hpp Normal file
View File

@@ -0,0 +1,74 @@
/**
* @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 <future>
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();
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;
};
}

219
components/ext_devs/ext_dev/src/lsm6dsowtr/maix_lsm6dsowtr.cpp Normal file
View File

@@ -0,0 +1,219 @@
/**
* @author lxowalle
* @copyright Sipeed Ltd 2024-
* @license Apache 2.0
* @update 2025-09-02: Add framework, create this file.
*/
#include "maix_lsm6dsowtr.hpp"
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
namespace maix::ext_dev::lsm6dsowtr {
#define GYRO_DEVICE_NAME "lsm6dsox_gyro"
#define ACCEL_DEVICE_NAME "lsm6dsox_accel"
static std::string __load_file_to_string(std::string path) {
FILE* fp = fopen(path.c_str(), "r");
if (!fp) {
perror("fopen");
return "";
}
std::string content;
char buffer[128];
while (fgets(buffer, sizeof(buffer), fp)) {
content += buffer;
}
fclose(fp);
return content;
}
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;
}
}
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;
}
}
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 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;
}
}
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));
}
}
fclose(f);
idx ++;
} else {
break;
}
}
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, gyro_y * gyro_scale, gyro_z * gyro_scale};
}
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, accel_y * accel_scale, accel_z * accel_scale};
}
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;
}
}
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);
}
return ret;
}
}

28
components/ext_devs/ext_dev/src/maix_imu.cpp
View File

@@ -2,6 +2,7 @@
#include "maix_basic.hpp"
#include "maix_imu.hpp"
#include "maix_qmi8658.hpp"
#include "maix_lsm6dsowtr.hpp"
#include "maix_ahrs_type.hpp"
#include <nlohmann/json.hpp>
#include <fstream>
@@ -16,12 +17,14 @@ namespace maix::ext_dev::imu {
enum class driver_type
{
qmi8658
qmi8658,
lsm6dsowtr,
};
typedef struct {
union {
maix::ext_dev::qmi8658::QMI8658 *qmi8658;
maix::ext_dev::lsm6dsowtr::LSM6DSOWTR *lsm6dsowtr;
} driver;
double bias[6];
driver_type type;
@@ -100,12 +103,18 @@ IMU::IMU(std::string driver, int i2c_bus, int addr, int freq, imu::Mode mode, im
{
param->type = driver_type::qmi8658;
found = true;
} else if(sys::device_id() == "maixcam2") {
param->type = driver_type::lsm6dsowtr;
found = true;
}
}
else if(driver == "qmi8658")
{
param->type = driver_type::qmi8658;
found = true;
} else if (driver == "lsm6dsowtr") {
param->type = driver_type::lsm6dsowtr;
found = true;
}
if (!found)
@@ -125,7 +134,17 @@ IMU::IMU(std::string driver, int i2c_bus, int addr, int freq, imu::Mode mode, im
}
// log::info("load calibration data: {%f, %f, %f, %f, %f, %f}",
// param->bias[0], param->bias[1], param->bias[2], param->bias[3], param->bias[4], param->bias[5]);
param->driver.qmi8658 = new maix::ext_dev::qmi8658::QMI8658(i2c_bus, addr, freq, mode, acc_scale, acc_odr, gyro_scale, gyro_odr, block);
switch (param->type) {
case driver_type::qmi8658:
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);
break;
default:
throw err::Exception("Unsupported driver type");
break;
}
}
IMU::~IMU()
@@ -135,6 +154,9 @@ IMU::~IMU()
if (_driver == "qmi8658") {
delete param->driver.qmi8658;
param->driver.qmi8658 = NULL;
} else if (_driver == "lsm6dsowtr") {
delete param->driver.lsm6dsowtr;
param->driver.lsm6dsowtr = NULL;
}
free(_param);
_param = NULL;
@@ -147,6 +169,8 @@ std::vector<float> IMU::read()
imu_param_t *param = (imu_param_t *)_param;
if (param->type == driver_type::qmi8658) {
out = param->driver.qmi8658->read();
} else if (param->type == driver_type::lsm6dsowtr) {
out = param->driver.lsm6dsowtr->read();
}
return out;
}

4
examples/ext_dev_imu_ahrs/main/src/main.cpp
View File

@@ -116,7 +116,7 @@ void draw_image(image::Image &img, tensor::Vector3f &angle, bool dir_cam)
void show_msg(display::Display &disp, std::string &msg)
{
auto img = image::Image(disp.width(), disp.height());
auto img = image::Image(disp.width(), disp.height(), image::FMT_RGB888, image::COLOR_BLACK);
auto size = image::string_size(msg, 1.5);
img.draw_string((img.width() - size.width()) / 2, (img.height() - size.height()) / 2, msg, image::COLOR_WHITE, 1.5);
disp.show(img);
@@ -192,7 +192,7 @@ int _main(int argc, char* argv[])
// printf("%s\n", temp_char);
// show on image
auto img = image::Image(disp.width(), disp.height());
auto img = image::Image(disp.width(), disp.height(), image::FMT_RGB888, image::COLOR_BLACK);
img.draw_string(2, 4, temp_char, image::COLOR_WHITE, 1.5);
snprintf(temp_char, sizeof(temp_char), "dt: %3dms, temp: %4.1f", (int)(dt * 1000), data.temp);
img.draw_string(2, 4+32, temp_char, image::COLOR_WHITE, 1.5);

39
examples/ext_dev_imu_read_raw/main/src/main.cpp
View File

@@ -11,11 +11,13 @@ static void helper(void)
log::info(
"==================================\r\n"
"Please input command:\r\n"
"0 <path> : read imu data and save in gcsv format\r\n"
"0 : read imu data and print\r\n"
"1 : caculate calibration\r\n"
"2 <path> : read imu data and save in gcsv format\r\n"
"\r\n"
"Example: ./maix_imu 0 output.mp4\r\n"
"Example: ./maix_imu 0\r\n"
"Example: ./maix_imu 1\r\n"
"Example: ./maix_imu 2 <output.mp4>\r\n"
"==================================\r\n");
}
@@ -37,7 +39,32 @@ int _main(int argc, char* argv[])
switch (cmd) {
case 0:
{
imu::IMU imu("qmi8658");
imu::IMU imu("default");
while (!app::need_exit()) {
auto res = imu.read();
printf("\n");
log::info("------------------------");
printf("acc x: %f\t", res[0]);
printf("acc y: %f\t", res[1]);
printf("acc z: %f\n", res[2]);
printf("gyro x: %f (%f)\t", res[3], res[3] * M_PI / 180);
printf("gyro y: %f (%f)\t", res[4], res[4] * M_PI / 180);
printf("gyro z: %f (%f)\n", res[5], res[5] * M_PI / 180);
printf("temp: %f\n", res[6]);
log::info("------------------------\n");
time::sleep_ms(500);
}
}
break;
case 1:
{
imu::IMU imu("default");
imu.calculate_calibration();
}
break;
case 2:
{
imu::IMU imu("default");
imu::Gcsv gcsv = imu::Gcsv();
double t_scale = 0.001;
double g_scale = ((double)1024 * M_PI / 180) / 32768;
@@ -65,12 +92,6 @@ int _main(int argc, char* argv[])
}
}
break;
case 1:
{
imu::IMU imu("qmi8658");
imu.calculate_calibration();
}
break;
default:
helper();
break;