diff --git a/Kconfig b/Kconfig index 5692df8d..9ab02376 100644 --- a/Kconfig +++ b/Kconfig @@ -13,6 +13,10 @@ menu "external device Components Configuration" osource "${SDK_PATH}/components/ext_devs/*/Kconfig" endmenu +menu "Algorithm Components Configuration" + osource "${SDK_PATH}/components/algo/*/Kconfig" +endmenu + menu "Extra Components Configuration" osource "${MAIXCDK_EXTRA_COMPONENTS_PATH}/*/Kconfig" # components released with python package format, installed by pip install maixcdk-xxxx diff --git a/components/algo/ahrs/CMakeLists.txt b/components/algo/ahrs/CMakeLists.txt new file mode 100644 index 00000000..50d41d80 --- /dev/null +++ b/components/algo/ahrs/CMakeLists.txt @@ -0,0 +1,71 @@ + +list(APPEND ADD_INCLUDE "include") +append_srcs_dir(ADD_SRCS "src") +list(APPEND ADD_REQUIREMENTS basic) + +register_component() + +# Config enable component2 or not in Kconfig +################# Add include ################# +# list(APPEND ADD_INCLUDE "include" +# ) +# list(APPEND ADD_PRIVATE_INCLUDE "include_private") +############################################### + +############## Add source files ############### +# list(APPEND ADD_SRCS "src/lib2.c" +# ) +# FILE(GLOB_RECURSE EXTRA_SRC "src/*.c") +# FILE(GLOB EXTRA_SRC "src/*.c") +# list(APPEND ADD_SRCS ${EXTRA_SRC}) +# aux_source_directory(src ADD_SRCS) # collect all source file in src dir, will set var ADD_SRCS +# append_srcs_dir(ADD_SRCS "src") # append source file in src dir to var ADD_SRCS +# list(REMOVE_ITEM COMPONENT_SRCS "src/test.c") +# set(ADD_ASM_SRCS "src/asm.S") +# list(APPEND ADD_SRCS ${ADD_ASM_SRCS}) +# SET_PROPERTY(SOURCE ${ADD_ASM_SRCS} PROPERTY LANGUAGE C) # set .S ASM file as C language +# SET_SOURCE_FILES_PROPERTIES(${ADD_ASM_SRCS} PROPERTIES COMPILE_FLAGS "-x assembler-with-cpp -D BBBBB") +############################################### + + +###### Add required/dependent components ###### +# list(APPEND ADD_REQUIREMENTS basic ini) +############################################### + +###### Add link search path for requirements/libs ###### +# list(APPEND ADD_LINK_SEARCH_PATH "${CONFIG_TOOLCHAIN_PATH}/lib") +# list(APPEND ADD_REQUIREMENTS pthread m) # add system libs, pthread and math lib for example here +# set (OpenCV_DIR opencv/lib/cmake/opencv4) +# find_package(OpenCV REQUIRED) +############################################### + +############ Add static libs ################## +# list(APPEND ADD_STATIC_LIB "lib/libtest.a") +############################################### + +############ Add dynamic libs ################## +# list(APPEND ADD_DYNAMIC_LIB "lib/arch/v831/libmaix_nn.so" +# "lib/arch/v831/libmaix_cam.so" +# ) +############################################### + +#### Add compile option for this component #### +#### Just for this component, won't affect other +#### modules, including component that depend +#### on this component +# list(APPEND ADD_DEFINITIONS_PRIVATE -DAAAAA=1) + +#### Add compile option for this component +#### Add components that depend on this component +# list(APPEND ADD_DEFINITIONS -DAAAAA222=1 +# -DAAAAA333=1) +############################################### + +############ Add static libs ################## +#### Update parent's variables like CMAKE_C_LINK_FLAGS +# set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--start-group libmaix/libtest.a -ltest2 -Wl,--end-group" PARENT_SCOPE) +############################################### + +# register component, DYNAMIC or SHARED flags will make component compiled to dynamic(shared) lib +# register_component() + diff --git a/components/algo/ahrs/Kconfig b/components/algo/ahrs/Kconfig new file mode 100644 index 00000000..eeb8c5c7 --- /dev/null +++ b/components/algo/ahrs/Kconfig @@ -0,0 +1,3 @@ + +# menu "algo component configuration" + diff --git a/components/algo/ahrs/component.py b/components/algo/ahrs/component.py new file mode 100644 index 00000000..ecfe5237 --- /dev/null +++ b/components/algo/ahrs/component.py @@ -0,0 +1,15 @@ + + +# def add_requirements(platform : str, find_dirs : list): +# if platform == "maixcam": +# return [ +# "nn", +# ] +# elif platform == "maixcam2": +# return [ +# "nn", +# "json" +# ] +# else: +# raise Exception("maixcam_lib component.py not add this platform support yet") + diff --git a/components/algo/ahrs/include/maix_ahrs_mahony.hpp b/components/algo/ahrs/include/maix_ahrs_mahony.hpp new file mode 100644 index 00000000..580dcc03 --- /dev/null +++ b/components/algo/ahrs/include/maix_ahrs_mahony.hpp @@ -0,0 +1,316 @@ +/* + * Mahony AHRS + * @author Neucrack + * @license Apache2.0 + * @update 2016.7.1 by Neucrack, add source code. + * 2025.7.4 by Neucrack, optimize for MaixPy/MaixCDK + * +*/ +#pragma once + +#include "math.h" +#include "maix_type_vector3.hpp" +#include "maix_ahrs_type.hpp" + +namespace maix::ahrs +{ + /** + * class MahonyAHRS for Attitude Estimation from IMU data, a Complementary Filter, + * support accelerometer, gyroscope and magnetometer fusion. + * @maixpy maix.ahrs.MahonyAHRS + */ + class MahonyAHRS + { + private: + //! Auxiliary variables to reduce number of repeated operations + float q0, q1, q2 , q3 ; /** quaternion of sensor frame relative to auxiliary frame */ + float dq0, dq1, dq2 , dq3; /** quaternion of sensor frame relative to auxiliary frame */ + float gyro_bias[3]; /** bias estimation */ + float q0q0, q0q1, q0q2, q0q3; + float q1q1, q1q2, q1q3; + float q2q2, q2q3; + float q3q3; + unsigned char bFilterInit; + + public: + /** + * P of PI controller, a larger P (proportional gain) leads to faster response, + * but it increases the risk of overshoot and oscillation. + * @maixpy maix.ahrs.MahonyAHRS.kp + */ + float kp; + /** + * I of PI controller, a larger I (integral gain) helps to eliminate steady-state errors more quickly, + * but it can accumulate error over time, potentially causing instability or slow drift. + * @maixpy maix.ahrs.MahonyAHRS.ki + */ + float ki; + + public: + /** + * class MahonyAHRS constructor. + * @param kp P of PI controller, a larger P (proportional gain) leads to faster response, + * but it increases the risk of overshoot and oscillation. + * @param ki I of PI controller, a larger I (integral gain) helps to eliminate steady-state errors more quickly, + * but it can accumulate error over time, potentially causing instability or slow drift. + * @maixcdk maix.ahrs.MahonyAHRS.MahonyAHRS + * @maixpy maix.ahrs.MahonyAHRS.__init__ + */ + MahonyAHRS(float kp,float ki) + { + this->kp = kp; + this->ki = ki; + bFilterInit = 0; + memset(gyro_bias, 0, sizeof(gyro_bias)); + } + + /** + * Initialize by accelerometer and magnetometer(optional). + * If you not call this method mannually, get_angle and update method will automatically call it. + * @param ax x axis of accelerometer, unit is g or raw data. + * @param ay y axis of accelerometer, unit is g or raw data. + * @param ax z axis of accelerometer, unit is g or raw data. + * @param mx x axis of magnetometer, unit is uT or raw data, mx, my, mz all 0 means not use magnetometer. + * @param my y axis of magnetometer, unit is uT or raw data, mx, my, mz all 0 means not use magnetometer. + * @param mz z axis of magnetometer, unit is uT or raw data, mx, my, mz all 0 means not use magnetometer. + * @maixpy maix.ahrs.MahonyAHRS.init + */ + void init(float ax, float ay, float az, float mx = 0, float my = 0, float mz = 0) + { + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(ay, az); + initialPitch = atan2(ax, az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + // auxillary variables to reduce number of repeated operations, for 1st pass + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + bFilterInit = 1; + } + + /** + * Update angles by accelerometer, gyroscope and magnetometer(optional). + * get_angle method will automatically call it. + * @param ax x axis of accelerometer, unit is g or raw data. + * @param ay y axis of accelerometer, unit is g or raw data. + * @param ax z axis of accelerometer, unit is g or raw data. + * @param ax x axis of gyroscope, unit is rad/s. + * @param ay y axis of gyroscope, unit is rad/s. + * @param ax z axis of gyroscope, unit is rad/s. + * @param mx x axis of magnetometer, unit is uT or raw data, mx, my, mz all 0 means not use magnetometer. + * @param my y axis of magnetometer, unit is uT or raw data, mx, my, mz all 0 means not use magnetometer. + * @param mz z axis of magnetometer, unit is uT or raw data, mx, my, mz all 0 means not use magnetometer. + * @param dt Delta time between two times call update method. + * @maixpy maix.ahrs.MahonyAHRS.update + */ + void update(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float dt) + { + float recipNorm; + float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; + float twoKp = this->kp * 2; + float twoKi = this->ki * 2; + + // Make filter converge to initial solution faster + // This function assumes you are in static position. + // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen. + if(bFilterInit == 0) { + init(ax,ay,az,mx,my,mz); + return; + } + + //! If magnetometer measurement is available, use it. + if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { + float hx, hy, hz, bx, bz; + float halfwx, halfwy, halfwz; + + // Normalise magnetometer measurement + // Will sqrt work better? PX4 system is powerful enough? + recipNorm = 1.0 / sqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Reference direction of Earth's magnetic field + hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); + hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); + hz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); + bx = sqrt(hx * hx + hy * hy); + bz = hz; + + // Estimated direction of magnetic field + halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); + halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); + halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); + + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += (my * halfwz - mz * halfwy); + halfey += (mz * halfwx - mx * halfwz); + halfez += (mx * halfwy - my * halfwx); + + } + + //增加一个条件： 加速度的模量与G相差不远时。 0.75*G < normAcc < 1.25*G + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) + { + float halfvx, halfvy, halfvz; + + // Normalise accelerometer measurement + //归一化，得到单位加速度 + recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az); + + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity and magnetic field + halfvx = q1q3 - q0q2; + halfvy = q0q1 + q2q3; + halfvz = q0q0 - 0.5f + q3q3; + + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += ay * halfvz - az * halfvy; + halfey += az * halfvx - ax * halfvz; + halfez += ax * halfvy - ay * halfvx; + // DEBUG_LOG<<"\t\t\t\t\t\t"< 0.0f) { + gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki + gyro_bias[1] += twoKi * halfey * dt; + gyro_bias[2] += twoKi * halfez * dt; + + // apply integral feedback + gx += gyro_bias[0]; + gy += gyro_bias[1]; + gz += gyro_bias[2]; + } + else { + gyro_bias[0] = 0.0f; // prevent integral windup + gyro_bias[1] = 0.0f; + gyro_bias[2] = 0.0f; + } + + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + // DEBUG_LOG<<"\t\t\t\t\t\t"< #include +#include "maix_type_vector3.hpp" + namespace maix { /** diff --git a/components/basic/include/maix_type_vector3.hpp b/components/basic/include/maix_type_vector3.hpp new file mode 100644 index 00000000..c233c085 --- /dev/null +++ b/components/basic/include/maix_type_vector3.hpp @@ -0,0 +1,271 @@ +#pragma once + +#include "math.h" + +namespace maix +{ + template + class Vector3 + { + public: + T x,y,z; + public: + //defualt constructor + Vector3() { x = 0; y = 0; z = 0; } + // setting ctor + Vector3(T x0, T y0, T z0) : x(x0), y(y0), z(z0) {} + //"()" overload + void operator()(T x0, T y0, T z0){x= x0; y= y0; z= z0;} + //"==" overload + bool operator==(const Vector3 &v){return (x==v.x && y==v.y && z==v.z); } + //"!=" overload + bool operator!=(const Vector3 &v){return (x!=v.x || y!=v.y || z!=v.z); } + // "-" negation overload + Vector3 operator-(void) const { return Vector3(-x,-y,-z);} + //"+" addition overload + Vector3 operator+(const Vector3 &v) const { return Vector3(x+v.x, y+v.y, z+v.z); } + //"-" subtraction overload + Vector3 operator-(const Vector3 &v) const { return Vector3(x-v.x, y-v.y, z-v.z); } + //"*" multiply overload + Vector3 operator*(const T n)const { return Vector3(x*n, y*n, z*n); } + //"/" divsion overload + Vector3 operator/(const T n)const { return Vector3(x/n, y/n, z/n); } + //"=" + Vector3 &operator=(const Vector3 &v){x=v.x; y=v.y; z=v.z; return *this;} + //"+=" overload + Vector3 &operator+=(const Vector3 &v) { x+=v.x; y+=v.y; z+=v.z; return *this; } + //"-=" overload + Vector3 &operator-=(const Vector3 &v) { x-=v.x; y-=v.y; z-=v.z; return *this; } + //"*=" overload + Vector3 &operator*=(const T n) { x*=n; y*=n; z*=n; return *this; } + // uniform scaling + Vector3 &operator/=(const T n) { x/=n; y/=n; z/=n; return *this; } + //dot product + T operator*(const Vector3 &v) const { return x*v.x + y*v.y + z*v.z; } + //cross product + Vector3 operator %(const Vector3 &v)const { return Vector3(y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x);} + // gets the length of this vector squared + T LengthSquared() const { return (T)(*this * *this); } + // gets the length of this vector + float Length(void) const { return (T)sqrt(*this * *this); } + // normalizes this vector + void Normalize() { *this/=Length(); } + // zero the vector + void Zero() { x = y = z = 0.0; } + // returns the normalized version of this vector + Vector3 Normalized() const { return *this/Length(); } + // check if any elements are NAN + bool IsNan(void) { return isnan(x) || isnan(y) || isnan(z); } + // check if any elements are infinity + bool IsInf(void) { return isinf(x) || isinf(y) || isinf(z); } + + }; + + /** + * Vector3 float type. + * @maixpy maix.Vector3f + */ + class Vector3f : public Vector3 // we use class for we want to generate maixpy API, and the tool not suppoort using yet. + { + public: + /** + * default constructor + * @maixcdk maix.Vector3f.Vector3f + */ + Vector3f() : Vector3() {} + + /** + * Construct Vector3f with 3 variables. + * @maixpy maix.Vector3f.__init__ + * @maixcdk maix.Vector3f.Vector3f + */ + Vector3f(float x0, float y0, float z0) + : Vector3(x0, y0, z0) + {} + + /** + * member x. + * @maixpy maix.Vector3f.x + */ + // float x; + + /** + * member y. + * @maixpy maix.Vector3f.y + */ + // float y; + + /** + * member z. + * @maixpy maix.Vector3f.z + */ + // float z; + }; + + /** + * Vector3 int32_t type. + * @maixpy maix.Vector3i32 + */ + class Vector3i32 : public Vector3 // we use class for we want to generate maixpy API, and the tool not suppoort using yet. + { + public: + /** + * default constructor + * @maixcdk maix.Vector3i32.Vector3i32 + */ + Vector3i32() : Vector3() {} + + /** + * Construct Vector3i32 with 3 variables. + * @maixpy maix.Vector3i32.__init__ + * @maixcdk maix.Vector3i32.Vector3i32 + */ + Vector3i32(int32_t x0, int32_t y0, int32_t z0) + : Vector3(x0, y0, z0) + {} + + /** + * member x. + * @maixpy maix.Vector3i32.x + */ + // int32_t x; + + /** + * member y. + * @maixpy maix.Vector3i32.y + */ + // int32_t y; + + /** + * member z. + * @maixpy maix.Vector3i32.z + */ + // int32_t z; + }; + + /** + * Vector3 uint32_t type. + * @maixpy maix.Vector3u32 + */ + class Vector3u32 : public Vector3 // we use class for we want to generate maixpy API, and the tool not suppoort using yet. + { + public: + /** + * default constructor + * @maixcdk maix.Vector3u32.Vector3u32 + */ + Vector3u32() : Vector3() {} + + /** + * Construct Vector3u32 with 3 variables. + * @maixpy maix.Vector3u32.__init__ + * @maixcdk maix.Vector3u32.Vector3u32 + */ + Vector3u32(uint32_t x0, uint32_t y0, uint32_t z0) + : Vector3(x0, y0, z0) + {} + + /** + * member x. + * @maixpy maix.Vector3u32.x + */ + // uint32_t x; + + /** + * member y. + * @maixpy maix.Vector3u32.y + */ + // uint32_t y; + + /** + * member z. + * @maixpy maix.Vector3u32.z + */ + // uint32_t z; + }; + + /** + * Vector3 int16 type. + * @maixpy maix.Vector3i16 + */ + class Vector3i16 : public Vector3 // we use class for we want to generate maixpy API, and the tool not suppoort using yet. + { + public: + /** + * default constructor + * @maixcdk maix.Vector3i16.Vector3i16 + */ + Vector3i16() : Vector3() {} + + /** + * Construct Vector3i16 with 3 variables. + * @maixpy maix.Vector3i16.__init__ + * @maixcdk maix.Vector3i16.Vector3i16 + */ + Vector3i16(int16_t x0, int16_t y0, int16_t z0) + : Vector3(x0, y0, z0) + {} + + /** + * member x. + * @maixpy maix.Vector3i16.x + */ + // int16_t x; + + /** + * member y. + * @maixpy maix.Vector3i16.y + */ + // int16_t y; + + /** + * member z. + * @maixpy maix.Vector3i16.z + */ + // int16_t z; + }; + + /** + * Vector3 uint16 type. + * @maixpy maix.Vector3u16 + */ + class Vector3u16 : public Vector3 // we use class for we want to generate maixpy API, and the tool not suppoort using yet. + { + public: + /** + * default constructor + * @maixcdk maix.Vector3u16.Vector3u16 + */ + Vector3u16() : Vector3() {} + + /** + * Construct Vector3u16 with 3 variables. + * @maixpy maix.Vector3u16.__init__ + * @maixcdk maix.Vector3u16.Vector3u16 + */ + Vector3u16(uint16_t x0, uint16_t y0, uint16_t z0) + : Vector3(x0, y0, z0) + {} + + /** + * member x. + * @maixpy maix.Vector3u16.x + */ + // uint16_t x; + + /** + * member y. + * @maixpy maix.Vector3u16.y + */ + // uint16_t y; + + /** + * member z. + * @maixpy maix.Vector3u16.z + */ + // uint16_t z; + }; + +} //namespace maix + + diff --git a/components/basic/src/maix_log.cpp b/components/basic/src/maix_log.cpp index 6df965db..1c8d4ace 100644 --- a/components/basic/src/maix_log.cpp +++ b/components/basic/src/maix_log.cpp @@ -159,4 +159,9 @@ namespace maix::log va_end(args); } + void flush(log::LogLevel level) + { + fflush(stdout); + } + } // namespace maix::log diff --git a/components/ext_devs/ext_dev/CMakeLists.txt b/components/ext_devs/ext_dev/CMakeLists.txt index c982ec3c..75289cc7 100644 --- a/components/ext_devs/ext_dev/CMakeLists.txt +++ b/components/ext_devs/ext_dev/CMakeLists.txt @@ -20,7 +20,7 @@ append_srcs_dir(ADD_SRCS "src/axp2101") append_srcs_dir(ADD_SRCS "src/fp5510") append_srcs_dir(ADD_SRCS "src/cmap") -list(APPEND ADD_REQUIREMENTS basic peripheral) +list(APPEND ADD_REQUIREMENTS basic peripheral json ahrs) # if (PLATFORM_LINUX) # append_srcs_dir(ADD_SRCS "port/linux") diff --git a/components/ext_devs/ext_dev/include/maix_imu.hpp b/components/ext_devs/ext_dev/include/maix_imu.hpp index f2044786..b97d7416 100644 --- a/components/ext_devs/ext_dev/include/maix_imu.hpp +++ b/components/ext_devs/ext_dev/include/maix_imu.hpp @@ -85,6 +85,38 @@ enum class GyroOdr { GYRO_ODR_31_25, // Gyroscope ODR set to 31.25 Hz. }; +/** + * IMU data type. + * @maixpy maix.ext_dev.imu.IMUData + */ +class IMUData +{ +public: + /** + * accelerometer data + * @maixpy maix.ext_dev.imu.IMUData.acc + */ + maix::Vector3f acc; + + /** + * gyroscope data + * @maixpy maix.ext_dev.imu.IMUData.gyro + */ + maix::Vector3f gyro; + + /** + * magnetometer data + * @maixpy maix.ext_dev.imu.IMUData.mag + */ + maix::Vector3f mag; + + /** + * temperature data + * @maixpy maix.ext_dev.imu.IMUData.temp + */ + float temp; +}; + /** * QMI8656 driver class * @maixpy maix.ext_dev.imu.IMU @@ -110,8 +142,8 @@ public: IMU(std::string driver, int i2c_bus=-1, int addr=0x6B, int freq=400000, 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::AccOdr acc_odr=maix::ext_dev::imu::AccOdr::ACC_ODR_1000, + maix::ext_dev::imu::GyroScale gyro_scale=maix::ext_dev::imu::GyroScale::GYRO_SCALE_256DPS, maix::ext_dev::imu::GyroOdr gyro_odr=maix::ext_dev::imu::GyroOdr::GYRO_ODR_8000, bool block=true); ~IMU(); @@ -122,17 +154,66 @@ public: IMU& operator=(IMU&&) = delete; /** - * @brief Read data from IMU. + * @brief Read raw data from IMU, no calibration, recommend use read_all instead. * * @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. - * + * And the last one is temperature + * Unit acc: g/s + * Unit gyro: degree/s + * Unit temperate: degree * @maixpy maix.ext_dev.imu.IMU.read */ std::vector read(); /** - * @brief Caculate calibration, save calibration data to /maixapp/shart/imu_calibration + * read imu data from IMU. + * @param calib_gryo calibrate gyro data based on calib_gyro_data, you should load_calib_gyro first to load calib_gyro_data. + * @param radian gyro unit use rad/s instead of degree/s, default false(use degree/s). + * @return maix.ext_dev.imu.IMUData type. + * Unit acc: g/s + * Unit gyro: degree/s + * Unit temperate: degree + * @maixpy maix.ext_dev.imu.IMU.read_all + */ + ext_dev::imu::IMUData read_all(bool calib_gryo = true, bool radian = false); + + /** + * Calibrate gryo for time_ms long, get gryo bias. + * @param time_ms total time to collect data, unit is ms. + * @param interval_ms minimum read raw data interval, -1 means continues, 10ms mean >= 10ms. + * @param save_id Save calibration data to file or not, you can load by load_calib_gyro. + * Empty string means not save. By default value is "default", means save calibration as id "default". + * @maixpy maix.ext_dev.imu.IMU.calib_gyro + */ + Vector3f calib_gyro(uint64_t time_ms, int interval_ms = -1, const std::string &save_id = "default"); + + /** + * Load Gyro calibration from file, if not found all value will be 0. + * @param save_id saved id from valib_gyro, default is "default". + * @return If exist gyro calibration info return True else False. + * @maixpy maix.ext_dev.imu.IMU.calib_gyro_exists + */ + bool calib_gyro_exists(const std::string &save_id = "default"); + + /** + * Load Gyro calibration from file, if not found all value will be 0. + * @param save_id saved id from valib_gyro, default is "default". + * @maixpy maix.ext_dev.imu.IMU.load_calib_gyro + */ + Vector3f load_calib_gyro(const std::string &save_id = "default"); + + /** + * Save Gyro calibration to file. + * @param calib the calibration data you want to save. + * @param save_id saved id from valib_gyro, default is "default". + * @maixpy maix.ext_dev.imu.IMU.save_calib_gyro + */ + err::Err save_calib_gyro(const Vector3f &calib, const std::string &save_id = "default"); + + /** + * @brief !!!Depracated!!! + * Caculate calibration, save calibration data to /maixapp/share/misc/imu_calibration * @param time_ms caculate max time, unit:ms * @return err::Err * @@ -141,15 +222,22 @@ public: err::Err calculate_calibration(uint64_t time_ms = 30 * 1000); /** - * @brief Get calibration data + * @brief !!!Depracated!!! + * Get calibration data * @return return an array, format is [acc_x_bias, acc_y_bias, acc_z_bias, gyro_x_bias, gyro_y_bias, gyro_z_bias] * If the calibration file cannot be found, an empty array will be returned. * @maixpy maix.ext_dev.imu.IMU.get_calibration */ std::vector get_calibration(); + +public: + Vector3f calib_gyro_data; + private: void* _param; std::string _driver; + imu::Mode _mode; + bool _calib_gyro_loaded; }; typedef struct { diff --git a/components/ext_devs/ext_dev/src/maix_imu.cpp b/components/ext_devs/ext_dev/src/maix_imu.cpp index 063eb542..8886ce2b 100644 --- a/components/ext_devs/ext_dev/src/maix_imu.cpp +++ b/components/ext_devs/ext_dev/src/maix_imu.cpp @@ -2,17 +2,32 @@ #include "maix_basic.hpp" #include "maix_imu.hpp" #include "maix_qmi8658.hpp" +#include "maix_ahrs_type.hpp" +#include +#include -#define CALIBRATION_DATA_PATH "/maixapp/share/imu_calibration" +using json = nlohmann::json; + + +#define CALIBRATION_DATA_PATH "/maixapp/share/misc/imu_calibration" +#define CALIBRATION_DATA_PATH2 "/maixapp/share/misc/imu_calibration.json" namespace maix::ext_dev::imu { +enum class driver_type +{ + qmi8658 +}; + typedef struct { union { maix::ext_dev::qmi8658::QMI8658 *qmi8658; } driver; double bias[6]; + driver_type type; } imu_param_t; + + IMU::IMU(std::string driver, int i2c_bus, int addr, int freq, imu::Mode mode, imu::AccScale acc_scale, imu::AccOdr acc_odr, imu::GyroScale gyro_scale, imu::GyroOdr gyro_odr, bool block) { @@ -20,6 +35,20 @@ IMU::IMU(std::string driver, int i2c_bus, int addr, int freq, imu::Mode mode, im imu_param_t *param = (imu_param_t *)malloc(sizeof(imu_param_t)); err::check_null_raise(param, "Failed to malloc param"); + if(driver == "qmi8658") + { + param->type = driver_type::qmi8658; + } + else + { + free(param); + throw err::Exception(err::ERR_ARGS, "not support " + driver); + } + + _param = (void *)param; + _driver = driver; + _mode = mode; + memset(param->bias, 0, sizeof(param->bias)); std::vector calibration_data = get_calibration(); for (size_t i = 0; i < calibration_data.size(); i ++) { @@ -28,8 +57,6 @@ 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); - _param = (void *)param; - _driver = driver; } IMU::~IMU() @@ -49,12 +76,226 @@ std::vector IMU::read() { std::vector out; imu_param_t *param = (imu_param_t *)_param; - if (_driver == "qmi8658") { + if (param->type == driver_type::qmi8658) { out = param->driver.qmi8658->read(); } return out; } +ext_dev::imu::IMUData IMU::read_all(bool calib_gryo, bool radian) +{ + ext_dev::imu::IMUData res; + auto data = read(); + switch(_mode) + { + case Mode::ACC_ONLY: + res.acc.x = data[0]; + res.acc.y = data[1]; + res.acc.z = data[2]; + res.temp = data[3]; + break; + case Mode::GYRO_ONLY: + res.gyro.x = data[0]; + res.gyro.y = data[1]; + res.gyro.z = data[2]; + res.temp = data[3]; + break; + case Mode::DUAL: + res.acc.x = data[0]; + res.acc.y = data[1]; + res.acc.z = data[2]; + res.gyro.x = data[3]; + res.gyro.y = data[4]; + res.gyro.z = data[5]; + res.temp = data[6]; + break; + default: + throw err::Exception(err::ERR_NOT_IMPL); + break; + } + if(calib_gryo) + { + res.gyro.x -= calib_gyro_data.x; + res.gyro.y -= calib_gyro_data.y; + res.gyro.z -= calib_gyro_data.z; + } + if(radian) + { + res.gyro.x *= ahrs::DEG2RAD; + res.gyro.y *= ahrs::DEG2RAD; + res.gyro.z *= ahrs::DEG2RAD; + } + return res; +} + +Vector3f IMU::calib_gyro(uint64_t time_ms, int interval_ms, const std::string &save_id) +{ + uint64_t start_ms = time::ticks_ms(); + uint64_t last_ms = 0; + uint64_t last_print_ms = 0; + double total_bias[3] = {0}; + int count = 0; + + log::info0("calib_gyro now for %ld ms, !! don't move !! ...", time_ms); + while (!app::need_exit() && time::ticks_ms() - start_ms <= time_ms) + { + uint64_t now = time::ticks_ms(); + if ((int)(now - last_ms) >= interval_ms) + { + last_ms = now; + IMUData data = read_all(false, false); + + total_bias[0] += data.gyro.x; + total_bias[1] += data.gyro.y; + total_bias[2] += data.gyro.z; + + count++; + } + if (now - last_print_ms >= 500) + { + last_print_ms = now; + log::print(log::LogLevel::LEVEL_INFO, "."); + log::flush(); + } + } + log::print(log::LogLevel::LEVEL_INFO, "\n"); + + _calib_gyro_loaded = true; + if (count > 0) + { + calib_gyro_data.x = static_cast(total_bias[0] / count); + calib_gyro_data.y = static_cast(total_bias[1] / count); + calib_gyro_data.z = static_cast(total_bias[2] / count); + } + else + { + calib_gyro_data.x = 0; + calib_gyro_data.y = 0; + calib_gyro_data.z = 0; + } + log::info("calib_gyro done, x: %.2f, y: %.2f, z %.2f", calib_gyro_data.x, calib_gyro_data.y, calib_gyro_data.z); + + if(!save_id.empty()) + { + log::info("calib_gyro save ..."); + save_calib_gyro(calib_gyro_data, save_id); + log::info("calib_gyro save done"); + } + + return calib_gyro_data; +} + +bool IMU::calib_gyro_exists(const std::string &save_id) +{ + if (fs::exists(CALIBRATION_DATA_PATH2)) + { + std::ifstream f(CALIBRATION_DATA_PATH2); + try + { + json data = json::parse(f); + + if (data.contains(save_id) && data[save_id].contains("gyro")) + { + return true; + } + } + catch (const std::exception &e) + { + log::error("%s %s", "Failed to parse or load gyro calibration:", e.what()); + } + } + return false; +} + +Vector3f IMU::load_calib_gyro(const std::string &save_id) +{ + // load CALIBRATION_DATA_PATH2 with json + Vector3f calib; + calib.x = 0; + calib.y = 0; + calib.z = 0; + if (fs::exists(CALIBRATION_DATA_PATH2)) + { + std::ifstream f(CALIBRATION_DATA_PATH2); + try + { + json data = json::parse(f); + + if (data.contains(save_id) && data[save_id].contains("gyro")) + { + const auto &gyro = data[save_id]["gyro"]; + if (gyro.contains("x") && gyro.contains("y") && gyro.contains("z")) + { + calib.x = gyro["x"].get(); + calib.y = gyro["y"].get(); + calib.z = gyro["z"].get(); + } + } + } + catch (const std::exception &e) + { + log::error("%s %s", "Failed to parse or load gyro calibration:", e.what()); + } + } + + calib_gyro_data = calib; + _calib_gyro_loaded = true; + return calib; +} + +err::Err IMU::save_calib_gyro(const Vector3f &calib, const std::string &save_id) +{ + json data; + + if(save_id.empty()) + return err::ERR_ARGS; + + // 先尝试读取已有文件内容 + if (fs::exists(CALIBRATION_DATA_PATH2)) + { + std::ifstream f(CALIBRATION_DATA_PATH2); + try + { + data = json::parse(f); + } + catch (const std::exception &e) + { + log::error("%s %s", "Failed to parse calibration JSON:", e.what()); + data = json::object(); + } + } + else + { + data = json::object(); + } + + // 更新或添加 save_id 下的 gyro 数据 + data[save_id]["gyro"]["x"] = calib.x; + data[save_id]["gyro"]["y"] = calib.y; + data[save_id]["gyro"]["z"] = calib.z; + + // 写回文件 + auto dir = fs::dirname(CALIBRATION_DATA_PATH2); + if (!dir.empty() && !fs::exists(dir)) + { + if (fs::mkdir(dir) != err::ERR_NONE) + { + log::error("%s %s","Failed to create directory:", dir.c_str()); + return err::ERR_IO; + } + } + std::ofstream ofs(CALIBRATION_DATA_PATH2); + if (!ofs.is_open()) + { + log::error("%s %s", "Failed to open calibration file for writing:", CALIBRATION_DATA_PATH2); + return err::ERR_IO; + } + ofs << data.dump(4); // 4空格缩进，美化输出 + ofs.close(); + + return err::ERR_NONE; +} + err::Err IMU::calculate_calibration(uint64_t time_ms) { imu_param_t *param = (imu_param_t *)_param; @@ -71,7 +312,7 @@ err::Err IMU::calculate_calibration(uint64_t time_ms) } count ++; if (time::ticks_ms() - last_ms >= 1000) { - log::info("caculate %d/%d", (time::ticks_ms() - start_ms) / 1000, caculate_total_time / 1000); + log::info("calibrate %d/%d", (time::ticks_ms() - start_ms) / 1000, caculate_total_time / 1000); last_ms = time::ticks_ms(); } } @@ -115,7 +356,6 @@ std::vector IMU::get_calibration() std::vector bias(6); int count = 0; while (fgets(buffer, sizeof(buffer), f) != NULL) { - log::info("%s", buffer); bias[count ++] = atof(buffer); } fclose(f); diff --git a/examples/imu_ahrs/.gitignore b/examples/imu_ahrs/.gitignore new file mode 100644 index 00000000..7171eaac --- /dev/null +++ b/examples/imu_ahrs/.gitignore @@ -0,0 +1,9 @@ +build +dist +.config.mk +.flash.conf.json +data + +/CMakeLists.txt + +__pycache__ diff --git a/examples/imu_ahrs/README.md b/examples/imu_ahrs/README.md new file mode 100644 index 00000000..2ce5c828 --- /dev/null +++ b/examples/imu_ahrs/README.md @@ -0,0 +1,5 @@ +Hello World Project based on MaixCDK +==== + +Hello world example code for MaixCDK of Sipeed, build method please visit [MaixCDK](https://github.com/sipeed/MaixCDK). + diff --git a/examples/imu_ahrs/app.yaml b/examples/imu_ahrs/app.yaml new file mode 100644 index 00000000..6b0dde2b --- /dev/null +++ b/examples/imu_ahrs/app.yaml @@ -0,0 +1,12 @@ +id: imu_ahrs +name: IMU AHRS +name[zh]: 姿态解算 +version: 1.0.3 +#icon: assets/hello.png +author: Sipeed Ltd +desc: IMU AHRS +desc[zh]: IMU 姿态解算 +exclude: + - dist + - build + - .gitignore diff --git a/examples/imu_ahrs/main/CMakeLists.txt b/examples/imu_ahrs/main/CMakeLists.txt new file mode 100644 index 00000000..4c8bbdb0 --- /dev/null +++ b/examples/imu_ahrs/main/CMakeLists.txt @@ -0,0 +1,74 @@ +############### Add include ################### +list(APPEND ADD_INCLUDE "include" + ) +list(APPEND ADD_PRIVATE_INCLUDE "") +############################################### + +############ Add source files ################# +# list(APPEND ADD_SRCS "src/main.c" +# "src/test.c" +# ) +append_srcs_dir(ADD_SRCS "src") # append source file in src dir to var ADD_SRCS +# list(REMOVE_ITEM COMPONENT_SRCS "src/test2.c") +# FILE(GLOB_RECURSE EXTRA_SRC "src/*.c") +# FILE(GLOB EXTRA_SRC "src/*.c") +# list(APPEND ADD_SRCS ${EXTRA_SRC}) +# aux_source_directory(src ADD_SRCS) # collect all source file in src dir, will set var ADD_SRCS +# append_srcs_dir(ADD_SRCS "src") # append source file in src dir to var ADD_SRCS +# list(REMOVE_ITEM COMPONENT_SRCS "src/test.c") +# set(ADD_ASM_SRCS "src/asm.S") +# list(APPEND ADD_SRCS ${ADD_ASM_SRCS}) +# SET_PROPERTY(SOURCE ${ADD_ASM_SRCS} PROPERTY LANGUAGE C) # set .S ASM file as C language +# SET_SOURCE_FILES_PROPERTIES(${ADD_ASM_SRCS} PROPERTIES COMPILE_FLAGS "-x assembler-with-cpp -D BBBBB") +############################################### + +###### Add required/dependent components ###### +list(APPEND ADD_REQUIREMENTS basic ext_dev vision ahrs) +############################################### + +###### Add link search path for requirements/libs ###### +# list(APPEND ADD_LINK_SEARCH_PATH "${CONFIG_TOOLCHAIN_PATH}/lib") +# list(APPEND ADD_REQUIREMENTS pthread m) # add system libs, pthread and math lib for example here +# set (OpenCV_DIR opencv/lib/cmake/opencv4) +# find_package(OpenCV REQUIRED) +############################################### + +############ Add static libs ################## +# list(APPEND ADD_STATIC_LIB "lib/libtest.a") +############################################### + +#### Add compile option for this component #### +#### Just for this component, won't affect other +#### modules, including component that depend +#### on this component +# list(APPEND ADD_DEFINITIONS_PRIVATE -DAAAAA=1) + +#### Add compile option for this component +#### and components depend on this component +# list(APPEND ADD_DEFINITIONS -DAAAAA222=1 +# -DAAAAA333=1) +############################################### + +############ Add static libs ################## +#### Update parent's variables like CMAKE_C_LINK_FLAGS +# set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--start-group libmaix/libtest.a -ltest2 -Wl,--end-group" PARENT_SCOPE) +############################################### + +######### Add files need to download ######### +# list(APPEND ADD_FILE_DOWNLOADS "{ +# 'url': 'https://*****/abcde.tar.xz', +# 'urls': [], # backup urls, if url failed, will try urls +# 'sites': [], # download site, user can manually download file and put it into dl_path +# 'sha256sum': '', +# 'filename': 'abcde.tar.xz', +# 'path': 'toolchains/xxxxx', +# 'check_files': [] +# }" +# ) +# +# then extracted file in ${DL_EXTRACTED_PATH}/toolchains/xxxxx, +# you can directly use then, for example use it in add_custom_command +############################################## + +# register component, DYNAMIC or SHARED flags will make component compiled to dynamic(shared) lib +register_component() diff --git a/examples/imu_ahrs/main/Kconfig b/examples/imu_ahrs/main/Kconfig new file mode 100644 index 00000000..e69de29b diff --git a/examples/imu_ahrs/main/include/main.h b/examples/imu_ahrs/main/include/main.h new file mode 100644 index 00000000..45dcbb04 --- /dev/null +++ b/examples/imu_ahrs/main/include/main.h @@ -0,0 +1,3 @@ +#pragma once + + diff --git a/examples/imu_ahrs/main/src/main.cpp b/examples/imu_ahrs/main/src/main.cpp new file mode 100644 index 00000000..1320350c --- /dev/null +++ b/examples/imu_ahrs/main/src/main.cpp @@ -0,0 +1,253 @@ + +#include "maix_basic.hpp" +#include "main.h" +#include "maix_imu.hpp" +#include "maix_ahrs_mahony.hpp" +#include "maix_touchscreen.hpp" + +#include "maix_image.hpp" +#include "maix_image_cv.hpp" +#include "maix_display.hpp" +#include "opencv2/opencv.hpp" + +using namespace maix; +using namespace ext_dev; + +static void helper(void) +{ + log::info( + "==================================\r\n" + "Usage:\r\n" + "\t imu_ahrs " + "\t\tcalibrate:" + "\t\t\t0 : only run\r\n" + "\t\t\t1 : calibrate and run\r\n" + "==================================\r\n"); +} + +bool is_in_button(int x, int y, std::vector btn_pos) +{ + return x > btn_pos[0] && x < btn_pos[0] + btn_pos[2] && y > btn_pos[1] && y < btn_pos[1] + btn_pos[3]; +} + +std::vector render_pose(float pitch, float roll, float yaw, bool radian) +{ + if(!radian) + { + pitch *= ahrs::DEG2RAD; + roll *= ahrs::DEG2RAD; + yaw *= ahrs::DEG2RAD; + } + // 构建绕 x 轴的旋转矩阵 (pitch) + cv::Matx33f Rx(1, 0, 0, + 0, cos(pitch), -sin(pitch), + 0, sin(pitch), cos(pitch)); + + // 绕 y 轴的旋转矩阵 (roll) + cv::Matx33f Ry(cos(roll), 0, sin(roll), + 0, 1, 0, + -sin(roll), 0, cos(roll)); + + // 绕 z 轴的旋转矩阵 (yaw) + cv::Matx33f Rz(cos(yaw), -sin(yaw), 0, + sin(yaw), cos(yaw), 0, + 0, 0, 1); + + // 总旋转矩阵：Z-Y-X 顺序 + cv::Matx33f R = Rz * Ry * Rx; + + // 定义单位向量 + cv::Vec3f x_axis(1, 0, 0); + cv::Vec3f y_axis(0, 1, 0); + cv::Vec3f z_axis(0, 0, 1); + + // 旋转单位向量 + cv::Vec3f x_rot = R * x_axis; + cv::Vec3f y_rot = R * y_axis; + cv::Vec3f z_rot = R * z_axis; + + // 投影到 x-z 平面：只保留 x 和 z 分量 + std::vector projections; + projections.emplace_back(x_rot[0], x_rot[2]); + projections.emplace_back(y_rot[0], y_rot[2]); + projections.emplace_back(z_rot[0], z_rot[2]); + + return projections; +} + +void draw_image(image::Image &img, Vector3f &angle, bool dir_cam) +{ + float len = 0.35; + int min_edge = std::min(img.width(), img.height()); + float obj_len = min_edge * len; + float offset_x = img.width() * 0.5; + float offset_y = img.height() * 0.5; + auto v = render_pose(angle.x, angle.y, angle.z, false); + v[0].x *= obj_len; + v[1].x *= obj_len; + v[2].x *= obj_len; + v[0].y *= obj_len; + v[1].y *= obj_len; + v[2].y *= obj_len; + v[0].x += offset_x; + v[1].x += offset_x; + v[2].x += offset_x; + v[0].y = img.height() - v[0].y - offset_y; + v[1].y = img.height() - v[1].y - offset_y; + v[2].y = img.height() - v[2].y - offset_y; + if(dir_cam) + { + img.draw_line(offset_x, img.height() - offset_y, v[1].x, v[1].y, image::COLOR_RED, 5); + img.draw_line(v[2].x, v[2].y, v[0].x, v[0].y, image::COLOR_GRAY, 1); + img.draw_line(offset_x, img.height() - offset_y, v[0].x, v[0].y, image::COLOR_WHITE, 5); + img.draw_line(offset_x, img.height() - offset_y, v[2].x, v[2].y, image::COLOR_GREEN, 5); + } + else + { + img.draw_line(v[1].x, v[1].y, v[0].x, v[0].y, image::COLOR_GRAY, 1); + img.draw_line(offset_x, img.height() - offset_y, v[1].x, v[1].y, image::COLOR_RED, 5); + img.draw_line(offset_x, img.height() - offset_y, v[0].x, v[0].y, image::COLOR_WHITE, 5); + img.draw_line(offset_x, img.height() - offset_y, v[2].x, v[2].y, image::COLOR_GREEN, 5); + } + img.draw_string(v[0].x, v[0].y, "x", image::COLOR_WHITE, 1.5); + img.draw_string(v[1].x, v[1].y, "y", image::COLOR_RED, 1.5); + img.draw_string(v[2].x, v[2].y, "z", image::COLOR_GREEN, 1.5); +} + +void show_msg(display::Display &disp, std::string &msg) +{ + auto img = image::Image(disp.width(), disp.height()); + 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); +} + +int _main(int argc, char* argv[]) +{ + int calibrate = 0; + if (argc > 1) { + if (!strcmp(argv[1], "-h")) { + helper(); + return 0; + } else { + calibrate = atoi(argv[1]); + } + }; + + float kp = 2; + float ki = 0.01; + int pitch_offset = 0; + + display::Display disp; + touchscreen::TouchScreen ts; + int ts_x = 0, ts_y = 0; + bool ts_pressed = false; + bool ts_last_pressed = false; + std::string dir_btn_name = "x dir"; + std::string calib_btn_name = "Calibrate"; + auto dir_font_size = image::string_size(dir_btn_name); + auto calib_font_size = image::string_size(calib_btn_name); + std::vector calib_btn_disp_pos = {disp.width() - 100, disp.height() - 50, 100, 50}; + std::vector ret_btn_disp_pos = {0, disp.height() - 50, 100, 50}; + std::vector dir_btn_disp_pos = {disp.width() / 2 - dir_font_size.width() / 2 - 10, disp.height() - 50, dir_font_size.width() + 20 , 50}; + + imu::IMU imu("qmi8658"); + ahrs::MahonyAHRS ahrs(kp, ki); + if(calibrate == 1) + { + log::info("now calibrate, please don't move device"); + imu.calib_gyro(10000); + } + else + { + imu.load_calib_gyro(); + } + char temp_char[64]; + double last_time = time::ticks_s(); + while (!app::need_exit()) { + auto data = imu.read_all(true, true); // use calibrate value and unit rad/s. + double t = time::ticks_s(); + float dt = t - last_time; + auto angle = ahrs.get_angle(data.acc, data.gyro, data.mag, dt); + last_time = t; + + // make y axis same with camera(x rotate 90 degree) + angle.x -= pitch_offset; + + // print + // ^z / y(front) + // | / + // | / + // . ————————> x(right) + snprintf(temp_char, sizeof(temp_char), "pitch: %6.2f, roll: %6.2f, yaw: %6.2f", angle.x, angle.y, angle.z); + // printf("%s\n", temp_char); + + // show on image + auto img = image::Image(disp.width(), disp.height()); + 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); + draw_image(img, angle, pitch_offset == 90); + + // draw button + img.draw_rect(calib_btn_disp_pos[0], calib_btn_disp_pos[1], calib_btn_disp_pos[2], calib_btn_disp_pos[3], image::COLOR_WHITE, 2); + img.draw_string(calib_btn_disp_pos[0] + 10 , calib_btn_disp_pos[1] + (calib_btn_disp_pos[3] - calib_font_size.height()) / 2, calib_btn_name); + img.draw_rect(ret_btn_disp_pos[0], ret_btn_disp_pos[1], ret_btn_disp_pos[2], ret_btn_disp_pos[3], image::COLOR_WHITE, 2); + img.draw_string(ret_btn_disp_pos[0] + 10 , ret_btn_disp_pos[1] + (ret_btn_disp_pos[3] - calib_font_size.height()) / 2, "< Exit"); + img.draw_rect(dir_btn_disp_pos[0], dir_btn_disp_pos[1], dir_btn_disp_pos[2], dir_btn_disp_pos[3], image::COLOR_WHITE, 2); + img.draw_string(dir_btn_disp_pos[0] + 10 , dir_btn_disp_pos[1] + (dir_btn_disp_pos[3] - calib_font_size.height()) / 2, dir_btn_name); + + disp.show(img); + + // check button event + ts.read(ts_x, ts_y, ts_pressed); + if (ts_pressed && is_in_button(ts_x, ts_y, ret_btn_disp_pos)) + { + break; + } + else if (ts_pressed && !ts_last_pressed && is_in_button(ts_x, ts_y, dir_btn_disp_pos)) + { + if(pitch_offset == 0) + pitch_offset = 90; + else + pitch_offset = 0; + } + else if (ts_pressed && !ts_last_pressed && is_in_button(ts_x, ts_y, calib_btn_disp_pos)) + { + int count = 6; + while(count -- > 0) + { + std::string msg = "Place on desk, don't move.\nStart in " + std::to_string(count) + "s"; + show_msg(disp, msg); + time::sleep(1); + } + log::info("now calibrate, please don't move device"); + std::string msg = "Calibrating, don't move, keep 10s"; + show_msg(disp, msg); + imu.calib_gyro(10000); + ahrs.reset(); + last_time = time::ticks_s(); + } + ts_last_pressed = ts_pressed; + + // time::sleep_ms(1); // release cpu for a while + + // make sure loop interval > 1ms + while(time::ticks_s() - last_time < 0.001) + time::sleep_us(100); + } + return 0; +} + +int main(int argc, char* argv[]) +{ + // Catch signal and process + sys::register_default_signal_handle(); + + // Use CATCH_EXCEPTION_RUN_RETURN to catch exception, + // if we don't catch exception, when program throw exception, the objects will not be destructed. + // So we catch exception here to let resources be released(call objects' destructor) before exit. + CATCH_EXCEPTION_RUN_RETURN(_main, -1, argc, argv); +} + + diff --git a/tools/cmake/build.py b/tools/cmake/build.py index 840fbf1b..03f80cda 100644 --- a/tools/cmake/build.py +++ b/tools/cmake/build.py @@ -131,6 +131,7 @@ def get_components_find_dirs(configs): os.path.join(configs["SDK_PATH"], "components"), os.path.join(configs["SDK_PATH"], "components", "3rd_party"), os.path.join(configs["SDK_PATH"], "components", "ext_devs"), + os.path.join(configs["SDK_PATH"], "components", "algo"), configs["PROJECT_PATH"], os.path.join(configs["PROJECT_PATH"], "components"), os.path.join(configs["PROJECT_PATH"], "..", "components"), diff --git a/tools/cmake/compile.cmake b/tools/cmake/compile.cmake index 78112231..0a3175b8 100644 --- a/tools/cmake/compile.cmake +++ b/tools/cmake/compile.cmake @@ -231,7 +231,7 @@ function(register_component) # Add requirements # get requirements from component.py - set(cmd COMMAND ${python} -u ${SDK_PATH}/tools/cmake/build.py get_requirements ${PLATFORM} ${component_name} ${component_dir} ${SDK_PATH}/components ${SDK_PATH}/components/3rd_party ${SDK_PATH}/components/ext_devs ${MAIXCDK_EXTRA_COMPONENTS_PATH} ${PY_PKG_COMPONENTS_PATH} ${PY_USR_PKG_COMPONENTS_PATH} ${PROJECT_SOURCE_DIR}/../components ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/components) + set(cmd COMMAND ${python} -u ${SDK_PATH}/tools/cmake/build.py get_requirements ${PLATFORM} ${component_name} ${component_dir} ${SDK_PATH}/components ${SDK_PATH}/components/3rd_party ${SDK_PATH}/components/ext_devs ${SDK_PATH}/components/algo ${MAIXCDK_EXTRA_COMPONENTS_PATH} ${PY_PKG_COMPONENTS_PATH} ${PY_USR_PKG_COMPONENTS_PATH} ${PROJECT_SOURCE_DIR}/../components ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/components) execute_process(${cmd} RESULT_VARIABLE cmd_res OUTPUT_VARIABLE component_requires) if(NOT cmd_res EQUAL 0) message(FATAL_ERROR "Get valid components failed") @@ -359,6 +359,7 @@ macro(project name) find_components(components_dirs components_kconfig_files kconfig_defaults_files_args found_main ${SDK_PATH}/components/*) find_components(components_dirs components_kconfig_files kconfig_defaults_files_args found_main ${SDK_PATH}/components/3rd_party/*) find_components(components_dirs components_kconfig_files kconfig_defaults_files_args found_main ${SDK_PATH}/components/ext_devs/*) + find_components(components_dirs components_kconfig_files kconfig_defaults_files_args found_main ${SDK_PATH}/components/algo/*) # Find components in custom components folder, register components if(MAIXCDK_EXTRA_COMPONENTS_PATH) find_components(components_dirs components_kconfig_files kconfig_defaults_files_args found_main ${MAIXCDK_EXTRA_COMPONENTS_PATH}/*) @@ -574,7 +575,7 @@ macro(project name) endif() # guess components used - set(cmd COMMAND ${python} -u ${SDK_PATH}/tools/cmake/build.py get_valid_components ${PLATFORM} ${SDK_PATH}/components ${SDK_PATH}/components/3rd_party ${SDK_PATH}/components/ext_devs ${MAIXCDK_EXTRA_COMPONENTS_PATH} ${PY_PKG_COMPONENTS_PATH} ${PY_USR_PKG_COMPONENTS_PATH} ${PROJECT_SOURCE_DIR}/../components ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/components) + set(cmd COMMAND ${python} -u ${SDK_PATH}/tools/cmake/build.py get_valid_components ${PLATFORM} ${SDK_PATH}/components ${SDK_PATH}/components/3rd_party ${SDK_PATH}/components/ext_devs ${SDK_PATH}/components/algo ${MAIXCDK_EXTRA_COMPONENTS_PATH} ${PY_PKG_COMPONENTS_PATH} ${PY_USR_PKG_COMPONENTS_PATH} ${PROJECT_SOURCE_DIR}/../components ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/components) execute_process(${cmd} RESULT_VARIABLE cmd_res OUTPUT_VARIABLE component_valid) if(NOT cmd_res EQUAL 0) message(FATAL_ERROR "Get valid components failed") diff --git a/tools/doc_tool/gen_api.py b/tools/doc_tool/gen_api.py index 40782a72..b24524c2 100644 --- a/tools/doc_tool/gen_api.py +++ b/tools/doc_tool/gen_api.py @@ -351,7 +351,7 @@ def get_func_def_info(code): if func_name[0] in ["*", "&"]: func_name = func_name[1:] return_type += " " + func_name[0] - if return_type.startswith("static") or return_type.startswith("extern"): + if return_type.startswith("static") or return_type.startswith("extern") or return_type.startswith("inline"): return_type = return_type.split(" ", 1)[1].strip() params_code = code[idx_param + 1:-1].strip() except_pair = {