mirror of
https://github.com/sipeed/MaixCDK.git
synced 2026-11-09 17:04:38 -06:00
add AHRS support
This commit is contained in:
Neucrack
4
Kconfig
4
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
|
||||
|
||||
71
components/algo/ahrs/CMakeLists.txt
Normal file
71
components/algo/ahrs/CMakeLists.txt
Normal file
@@ -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()
|
||||
|
||||
3
components/algo/ahrs/Kconfig
Normal file
3
components/algo/ahrs/Kconfig
Normal file
@@ -0,0 +1,3 @@
|
||||
|
||||
# menu "algo component configuration"
|
||||
|
||||
15
components/algo/ahrs/component.py
Normal file
15
components/algo/ahrs/component.py
Normal file
@@ -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")
|
||||
|
||||
316
components/algo/ahrs/include/maix_ahrs_mahony.hpp
Normal file
316
components/algo/ahrs/include/maix_ahrs_mahony.hpp
Normal file
@@ -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
|
||||
35
components/algo/ahrs/include/maix_ahrs_type.hpp
Normal file
35
components/algo/ahrs/include/maix_ahrs_type.hpp
Normal file
@@ -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
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -11,6 +11,8 @@
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "maix_type_vector3.hpp"
|
||||
|
||||
namespace maix
|
||||
{
|
||||
/**
|
||||
|
||||
271
components/basic/include/maix_type_vector3.hpp
Normal file
271
components/basic/include/maix_type_vector3.hpp
Normal file
@@ -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
|
||||
|
||||
|
||||
@@ -159,4 +159,9 @@ namespace maix::log
|
||||
va_end(args);
|
||||
}
|
||||
|
||||
void flush(log::LogLevel level)
|
||||
{
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
} // namespace maix::log
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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<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 {
|
||||
|
||||
@@ -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);
|
||||
|
||||
9
examples/imu_ahrs/.gitignore
vendored
Normal file
9
examples/imu_ahrs/.gitignore
vendored
Normal file
@@ -0,0 +1,9 @@
|
||||
build
|
||||
dist
|
||||
.config.mk
|
||||
.flash.conf.json
|
||||
data
|
||||
|
||||
/CMakeLists.txt
|
||||
|
||||
__pycache__
|
||||
5
examples/imu_ahrs/README.md
Normal file
5
examples/imu_ahrs/README.md
Normal file
@@ -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).
|
||||
|
||||
12
examples/imu_ahrs/app.yaml
Normal file
12
examples/imu_ahrs/app.yaml
Normal file
@@ -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
|
||||
74
examples/imu_ahrs/main/CMakeLists.txt
Normal file
74
examples/imu_ahrs/main/CMakeLists.txt
Normal file
@@ -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()
|
||||
0
examples/imu_ahrs/main/Kconfig
Normal file
0
examples/imu_ahrs/main/Kconfig
Normal file
3
examples/imu_ahrs/main/include/main.h
Normal file
3
examples/imu_ahrs/main/include/main.h
Normal file
@@ -0,0 +1,3 @@
|
||||
#pragma once
|
||||
|
||||
|
||||
253
examples/imu_ahrs/main/src/main.cpp
Normal file
253
examples/imu_ahrs/main/src/main.cpp
Normal file
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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"),
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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 = {
|
||||
|
||||
Reference in New Issue
Block a user