add AHRS support

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

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()
+

components/algo/ahrs/Kconfig

@@ -0,0 +1,3 @@
+
+# menu "algo component configuration"
+

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")
+

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"<<halfex<<"\t"<<halfey<<"\t"<<halfez<<"\n";
+            }
+
+            // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
+            if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
+                // Compute and apply integral feedback if enabled
+                if(twoKi > 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"<<gx<<"\t"<<gy<<"\t"<<gz<<"\n";
+            }
+
+            // Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
+            //! q_k = q_{k-1} + dt*\dot{q}
+            //! \dot{q} = 0.5*q \otimes P(\omega)
+            dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
+            dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
+            dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
+            dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
+
+            q0 += dt*dq0;
+            q1 += dt*dq1;
+            q2 += dt*dq2;
+            q3 += dt*dq3;
+
+            // Normalise quaternion
+            recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+            q0 *= recipNorm;
+            q1 *= recipNorm;
+            q2 *= recipNorm;
+            q3 *= recipNorm;
+
+            // Auxiliary variables to avoid repeated arithmetic
+            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;
+        }
+
+        /**
+          * Get angle by mahony complementary filter, will automatically call update method,
+          * and automatically call init in first time.
+          * @param acc accelerometer data, unit is g or raw data. maix.Vector3f type.
+          * @param gyro gyroscope data, unit can be rad/s or degree/s, if rad/s, arg radian should be true. maix.Vector3f type.
+          * @param mag magnetometer data, optional, if no magnetometer, set all value to 0. maix.Vector3f type.
+          * @param dt delta T of two time call get_anle, unit is second, float type.
+          * @param radian if gyro's unit is rad/s, set this arg to true, degree/s set to false.
+          * @return rotation angle data, maix.Vector3f type.
+          * @maixpy maix.ahrs.MahonyAHRS.get_angle
+          */
+        Vector3f get_angle(Vector3f acc, Vector3f gyro,Vector3f mag, float dt, bool radian = false)
+        {
+            Vector3f angle;
+
+            update(acc.x, acc.y, acc.z, gyro.x, gyro.y, gyro.z, mag.x, mag.y, mag.z, dt);
+
+            if(radian)
+            {
+                angle.y = asin(-2 * q1 * q3 + 2 * q0* q2); // pitch for x axis in front, or roll
+                angle.x = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1);
+                angle.z = atan2(2*(q0*q3+q1*q2), 1 - 2 * (q2*q2+q3*q3));
+            }
+            else
+            {
+                angle.y = asin(-2 * q1 * q3 + 2 * q0* q2)* RAD2DEG; // pitch for x axis in front, or roll
+                angle.x = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RAD2DEG;
+                angle.z = atan2(2*(q0*q3+q1*q2), 1 - 2 * (q2*q2+q3*q3)) * RAD2DEG;
+            }
+
+            return angle;
+        }
+
+        /**
+          * reset to not initialized status.
+          * @maixpy maix.ahrs.MahonyAHRS.reset
+          */
+        void reset()
+        {
+            bFilterInit = 0;
+        }
+
+    };
+}; //namespace maix::ahrs

components/algo/ahrs/include/maix_ahrs_type.hpp

@@ -0,0 +1,35 @@
+/*
+ * 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"
+
+
+namespace maix::ahrs
+{
+    /**
+      * Math PI
+      * @maixpy maix.ahrs.PI
+     */
+    const float PI = 3.14159265358979323846f;
+
+    /**
+      * angle unit radian to degree.
+      * @maixpy maix.ahrs.RAD2DEG
+     */
+    const float RAD2DEG = 180.0f / PI;
+
+    /**
+      * angle unit degree to radian.
+      * @maixpy maix.ahrs.DEG2RAD
+     */
+    const float DEG2RAD = PI / 180.0f;
+
+}; //namespace maix::ahrs

components/basic/include/maix_log.hpp

@@ -134,6 +134,13 @@ namespace maix::log
     */
     void print(log::LogLevel level, const char *fmt, ...);
 
+    /**
+      * flush log
+      * @param level log level, @see maix.log.LogLevel
+      * @maixcdk maix.log.flush
+     */
+    void flush(log::LogLevel level = log::LogLevel::LEVEL_NONE);
+
 } // namespace maix::log
 
 

components/basic/include/maix_type.hpp

@@ -11,6 +11,8 @@
 #include <stdbool.h>
 #include <string.h>
 
+#include "maix_type_vector3.hpp"
+
 namespace maix
 {
     /**

components/basic/include/maix_type_vector3.hpp

@@ -0,0 +1,271 @@
+#pragma once
+
+#include "math.h"
+
+namespace maix
+{
+    template <typename T>
+    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<T> &v){return (x==v.x && y==v.y && z==v.z); }
+        //"!=" overload
+        bool operator!=(const Vector3<T> &v){return (x!=v.x || y!=v.y || z!=v.z); }
+        // "-" negation overload
+        Vector3<T> operator-(void) const { return Vector3<T>(-x,-y,-z);}
+        //"+" addition overload
+        Vector3<T> operator+(const Vector3<T> &v) const { return Vector3<T>(x+v.x, y+v.y, z+v.z); }
+        //"-" subtraction overload
+        Vector3<T> operator-(const Vector3<T> &v) const { return Vector3<T>(x-v.x, y-v.y, z-v.z); }
+        //"*" multiply overload
+        Vector3<T> operator*(const T n)const { return Vector3<T>(x*n, y*n, z*n); }
+        //"/" divsion overload
+        Vector3<T> operator/(const T n)const { return Vector3<T>(x/n, y/n, z/n); }
+        //"=" 
+        Vector3<T> &operator=(const Vector3<T> &v){x=v.x; y=v.y; z=v.z; return *this;}
+        //"+=" overload
+        Vector3<T> &operator+=(const Vector3<T> &v) { x+=v.x; y+=v.y; z+=v.z; return *this; }
+        //"-=" overload
+        Vector3<T> &operator-=(const Vector3<T> &v) { x-=v.x; y-=v.y; z-=v.z; return *this; }
+        //"*=" overload
+        Vector3<T> &operator*=(const T n) {  x*=n; y*=n; z*=n;  return *this; }
+        // uniform scaling
+        Vector3<T> &operator/=(const T n) {  x/=n; y/=n; z/=n;  return *this; }
+        //dot product
+        T operator*(const Vector3<T> &v) const { return x*v.x + y*v.y + z*v.z;  }
+        //cross product
+        Vector3<T> operator %(const Vector3<T> &v)const { return Vector3<T>(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<T>  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<float> // 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<float>() {}
+
+        /**
+          * Construct Vector3f with 3 variables.
+          * @maixpy maix.Vector3f.__init__
+          * @maixcdk maix.Vector3f.Vector3f
+         */
+        Vector3f(float x0, float y0, float z0)
+        : Vector3<float>(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<int32_t> // 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<int32_t>() {}
+
+        /**
+          * Construct Vector3i32 with 3 variables.
+          * @maixpy maix.Vector3i32.__init__
+          * @maixcdk maix.Vector3i32.Vector3i32
+         */
+         Vector3i32(int32_t x0, int32_t y0, int32_t z0)
+        : Vector3<int32_t>(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<uint32_t> // 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<uint32_t>() {}
+
+         /**
+           * Construct Vector3u32 with 3 variables.
+           * @maixpy maix.Vector3u32.__init__
+           * @maixcdk maix.Vector3u32.Vector3u32
+          */
+          Vector3u32(uint32_t x0, uint32_t y0, uint32_t z0)
+         : Vector3<uint32_t>(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<int16_t> // 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<int16_t>() {}
+
+         /**
+           * Construct Vector3i16 with 3 variables.
+           * @maixpy maix.Vector3i16.__init__
+           * @maixcdk maix.Vector3i16.Vector3i16
+          */
+          Vector3i16(int16_t x0, int16_t y0, int16_t z0)
+         : Vector3<int16_t>(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<uint16_t> // 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<uint16_t>() {}
+
+         /**
+           * Construct Vector3u16 with 3 variables.
+           * @maixpy maix.Vector3u16.__init__
+           * @maixcdk maix.Vector3u16.Vector3u16
+          */
+          Vector3u16(uint16_t x0, uint16_t y0, uint16_t z0)
+         : Vector3<uint16_t>(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
+
+

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

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")

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::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_16DPS,
+            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<float> 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<double> get_calibration();
+
+public:
+    Vector3f calib_gyro_data;
+
 private:
     void* _param;
     std::string _driver;
+    imu::Mode _mode;
+    bool _calib_gyro_loaded;
 };
 
 typedef struct {

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 <nlohmann/json.hpp>
+#include <fstream>
 
-#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<double> 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<float> IMU::read()
 {
     std::vector<float> 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<float>(total_bias[0] / count);
+        calib_gyro_data.y = static_cast<float>(total_bias[1] / count);
+        calib_gyro_data.z = static_cast<float>(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<float>();
+                    calib.y = gyro["y"].get<float>();
+                    calib.z = gyro["z"].get<float>();
+                }
+            }
+        }
+        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<double> IMU::get_calibration()
     std::vector<double> bias(6);
     int count = 0;
     while (fgets(buffer, sizeof(buffer), f) != NULL) {
-        log::info("%s", buffer);
         bias[count ++] = atof(buffer);
     }
     fclose(f);

examples/imu_ahrs/.gitignore

@@ -0,0 +1,9 @@
+build
+dist
+.config.mk
+.flash.conf.json
+data
+
+/CMakeLists.txt
+
+__pycache__

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).
+

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

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

examples/imu_ahrs/main/include/main.h

@@ -0,0 +1,3 @@
+#pragma once
+
+

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 <calibrate>"
+    "\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<int> 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<cv::Point2f> 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<cv::Point2f> 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<int> calib_btn_disp_pos = {disp.width() - 100, disp.height() - 50, 100, 50};
+    std::vector<int> ret_btn_disp_pos = {0, disp.height() - 50, 100, 50};
+    std::vector<int> 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);
+}
+
+

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"),

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")

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 = {