How to use TImer of nrf52480

Products & Technology XIAO
1

Why in arduino, after I include the XIAO Nrff52480 board, and the header file
#include <NRF52TimerInterrupt.hpp>
#include <NRF52_ISR_Timer.hpp>
is included, the system still tells me that there is an error.That is

In file included from D:\arduio doucument\hudie\hudie.ino:2:0:
d:\arduio doucument\libraries\NRF52_TimerInterrupt\src/NRF52TimerInterrupt.hpp:86:4: error: #error This code is designed to run on Adafruit or Seeed nRF52 platform! Please check your Tools->Board setting.
   #error This code is designed to run on Adafruit or Seeed nRF52 platform! Please check your Tools->Board setting.
    ^~~~~
In file included from d:\arduio doucument\libraries\NRF52_TimerInterrupt\src/NRF52TimerInterrupt.hpp:118:0,
                 from D:\arduio doucument\hudie\hudie.ino:2:
d:\arduio doucument\libraries\NRF52_TimerInterrupt\src/TimerInterrupt_Generic_Debug.h:42:10: fatal error: Adafruit_TinyUSB.h: No such file or directory
 #include <Adafruit_TinyUSB.h>
          ^~~~~~~~~~~~~~~~~~~~
compilation terminated.
2

Hi there,

You may have selected the wrong version of the BSP. there are two in the list be sure to pick th ecorrect one, Some LIB’s only work with certain BSP’s (board support package) supplied by the vendors.
With the NRF you the Embed versions 2.9.x and the non-embed versions 1.1.1 for example. Try the other one.

HTH
GL :slight_smile: PJ :v:

common over site… no big deal! :+1:

3

Thank you for your answer.But I have tried Embed versions 2.9.3、2.9.2、2.9.1，they don’t work.And I didn’t try non-embed versions,because I need to make Something with high power consumption.

Below is my board I selected.

屏幕截图 2025-08-06 104544
屏幕截图 2025-08-06 1045441614×661 54 KB

屏幕截图 2025-08-06 104746
屏幕截图 2025-08-06 104746648×292 14.5 KB

4

Hi there,

so,

What these libraries do:

  1. NRF52TimerInterrupt
    Lets you create hardware-based timers using the NRF52’s internal timers (TIMER0–4).
    You can set precise periodic interrupts in microseconds or milliseconds.
  2. NRF52_ISR_Timer
    Uses a single hardware timer but allows you to schedule multiple software-based timers, each with its own interval and callback — like a multitimer manager.

Those are compatable with BSP 1.1.1 , Try that first.
then can you post the code you are using, USE the code tags above “</>” paste it in there. You get better reply’s too.

HTH
GL :slight_smile: PJ :v:

doesn’t matter, 1.1.1 will do the same.

5

Okay,below is the code of .ino,


#include <NRF52TimerInterrupt.hpp>
#include <NRF52_ISR_Timer.hpp>

#include <LSM6DS3.h>


#include "IMU.h"

NRF52Timer ITimer(NRF_TIMER_1);
volatile uint32_t nowtime = 0;//定时器积分参数

#define TIMER0_FREQ_HZ        10000 //定时器定时100us
//IMU积分定时器
void TimerHandler0()
{
  nowtime ++;
  // Doing something here inside ISR
}

//设置速度0-255
void Set_Speed(int PWML,int PWMR)
{
  if(PWML<0)
  {
    analogWrite(8,-PWML);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
  }
  else if(PWML>=0)
  {
    analogWrite(8,PWML);
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);
  }
    if(PWMR<0)
  {
    analogWrite(9,-PWMR);
    digitalWrite(10, LOW);
    digitalWrite(11, HIGH);
  }
  else if(PWMR>=0)
  {
    analogWrite(9,PWMR);
    digitalWrite(10, HIGH);
    digitalWrite(11, LOW);
  }
}

volatile long encoderCounterL =0; //This variable will increase or decreas depending on the rotation of encoder
volatile long encoderCounterR =0; //This variable will increase or decreas depending on the rotation of encoder

#define encoderPinLA 0
#define encoderPinLB 1
#define encoderPinRA 2
#define encoderPinRB 3

#define xianshu 1024
#define jianshubi 13.5
#define beipin 4

void setup() 
{

  /********
  *端口选择左8 ，AIN16,AIN27
  *右9，BIN110,BIN211
  *
  */
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  pinMode(8,OUTPUT);

  pinMode(9,OUTPUT);
  pinMode(10,OUTPUT);
  pinMode(11,OUTPUT);
   //编码器输入
   //做点击，0，1
   //右电机， 2，3
  pinMode(encoderPinLA,INPUT);
  pinMode(encoderPinLB,INPUT);
  attachInterrupt(digitalPinToInterrupt(encoderPinLA), doEncoderLA, CHANGE);//B rising pulse from encodenren activated ai1(). AttachInterrupt 1 isDigitalPin nr 3 on moust Arduino.
  attachInterrupt(digitalPinToInterrupt(encoderPinLB), doEncoderLB, CHANGE);

  pinMode(encoderPinRA,INPUT);
  pinMode(encoderPinRB,INPUT);
  attachInterrupt(digitalPinToInterrupt(encoderPinRA), doEncoderRA, CHANGE);//B rising pulse from encodenren activated ai1(). AttachInterrupt 1 isDigitalPin nr 3 on moust Arduino.
  attachInterrupt(digitalPinToInterrupt(encoderPinRB), doEncoderRB, CHANGE);

  //IMU初始化
  IMU_init();
  ITimer.attachInterrupt(TIMER0_FREQ_HZ, TimerHandler0)//imu积分定时器初始化
  delay(20);

  Set_Speed(127,127);//设置初始速度
}
int my_abs(int a)
{
  if(a>0)
  {
    return a;
  }
  else 
  {
    return -a;
  }
}
void loop() {

    if((encoderCounterL*0.667 >=xianshu/jianshubi/beipin)&& (encoderCounterR*0.667 >=xianshu/jianshubi/beipin))
    {
      Set_Speed(-127,-127);
      encoderCounterL = 0;
      encoderCounterR= 0;//计数清0
    }
    else if((encoderCounterL*0.667 <= -xianshu/jianshubi/beipin)&& (encoderCounterR*0.667 <= -xianshu/jianshubi/beipin))
    {
      Set_Speed(127,127);
      encoderCounterL = 0;
      encoderCounterR= 0;//计数清0
    }
}
void doEncoderLA(void)
{
  if (digitalRead(encoderPinLA) == HIGH)//上升沿
   { 

    if (digitalRead(encoderPinLB) == LOW) {  
      encoderCounterL = encoderCounterL + 1;         // CW
    } 
    else {
      encoderCounterL = encoderCounterL - 1;         // CCW
    }
  }
  else   //下降沿                                     
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderPinLB) == HIGH) {   
      encoderCounterL = encoderCounterL + 1;          // CW
    } 
    else {
      encoderCounterL = encoderCounterL - 1;          // CCW
    }
  }

}
void doEncoderLB(void)
{
   if (digitalRead(encoderPinLB) == HIGH)//上升沿
    {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoderPinLA) == HIGH) {  
      encoderCounterL = encoderCounterL + 1;         // CW
    } 
    else {
      encoderCounterL = encoderCounterL - 1;         // CCW
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderPinLA) == LOW) {   
      encoderCounterL = encoderCounterL + 1;          // CW
    } 
    else {
      encoderCounterL = encoderCounterL - 1;          // CCW
    }
  }

}
void doEncoderRA(void)
{
   if (digitalRead(encoderPinRA) == HIGH)//上升沿
   { 

    if (digitalRead(encoderPinRB) == LOW) {  
      encoderCounterR = encoderCounterR + 1;         // CW
    } 
    else {
      encoderCounterR = encoderCounterR - 1;         // CCW
    }
  }
  else   //下降沿                                     
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderPinRB) == HIGH) {   
      encoderCounterR = encoderCounterR + 1;          // CW
    } 
    else {
      encoderCounterR = encoderCounterR - 1;          // CCW
    }
  }
}
void doEncoderRB(void)
{
  if (digitalRead(encoderPinRB) == HIGH)//上升沿
    {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoderPinRA) == HIGH) {  
      encoderCounterR = encoderCounterR + 1;         // CW
    } 
    else {
      encoderCounterR = encoderCounterR - 1;         // CCW
    }
  }
  else
   { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoderPinRA) == LOW) {   
      encoderCounterR = encoderCounterR + 1;          // CW
    } 
    else {
      encoderCounterR = encoderCounterR - 1;          // CCW
    }
  }
}
6

HI there,

Ok , so looks like a solid start to something, targeting a real-time control task: motor drive, encoder feedback, and IMU timing — so you’re right to use hardware timers and interrupts.

I don’t have "IMU.h " not sure what you got going on there, but you missed a semicolon in the timer attach
ITimer.attachInterrupt(TIMER0_FREQ_HZ, TimerHandler0); // ← ADD THIS
Also the pins for PWM are not supported on 8 and 9 on BSP 1.1.1 you need to declare them manually for that or
replace analogWrite(pin, val); with analogWrite(PWM_CHANNEL, duty); if using NRF52_PWM library.

I asked our AI , and the interrupts could be an issue at high speed check it out…"
You’re using 4 encoder interrupts (2 per wheel), each running conditional logic and memory writes. That’s acceptable for low RPM, but at higher speed, this could:

  • Starve the CPU
  • Cause missed IMU sampling in TimerHandler0
  • Break BLE, Serial, or other timing-sensitive code

Instead of dual A/B interrupts per encoder, you can reduce to 1 interrupt per wheel (on A) and check B inside the ISR.

For example:

attachInterrupt(digitalPinToInterrupt(encoderPinLA), encoderISR_L, CHANGE);

And inside encoderISR_L():

if (digitalRead(encoderPinLA) == digitalRead(encoderPinLB)) {
  encoderCounterL++;
} else {
  encoderCounterL--;
}

Saves 2 ISR entries per transition = more stable.
YMMV :v:
If using IMU.h from Seeed, confirm you are calling:

IMU.readAcceleration(x, y, z);
IMU.readGyroscope(gx, gy, gz);

I’ll leave it there , I see one or two more optimizations you could make.
Also there is a version of that LIB for embed , if you didn’t know.

HTH
GL :slight_smile: PJ :v:

check out the demo I have on here for Park, Motion, and Fall and also orientation with IMU, It’s an Awesome sensor (search Park IMU) :+1:

7

I haved used the board xiao mrf52840.But It brings bugs that weren’t there before:

In file included from D:\arduio doucument\hudie\IMU.c:1:0:
d:\arduio doucument\libraries\Seeed_Arduino_LSM6DS3/LSM6DS3.h:51:1: error: unknown type name 'class'
 class LSM6DS3Core {
 ^~~~~
d:\arduio doucument\libraries\Seeed_Arduino_LSM6DS3/LSM6DS3.h:51:19: error: expected '=', ',', ';', 'asm' or '__attribute__' before '{' token
 class LSM6DS3Core {
                   ^
d:\arduio doucument\libraries\Seeed_Arduino_LSM6DS3/LSM6DS3.h:92:3: error: expected specifier-qualifier-list before 'public'
   public:
   ^~~~~~
d:\arduio doucument\libraries\Seeed_Arduino_LSM6DS3/LSM6DS3.h:134:1: error: unknown type name 'class'
 class LSM6DS3 : public LSM6DS3Core {
 ^~~~~
d:\arduio doucument\libraries\Seeed_Arduino_LSM6DS3/LSM6DS3.h:134:15: error: expected '=', ',', ';', 'asm' or '__attribute__' before ':' token
 class LSM6DS3 : public LSM6DS3Core {
               ^
D:\arduio doucument\hudie\IMU.c:5:1: error: unknown type name 'LSM6DS3'
In file included from D:\arduio doucument\hudie\IMU.c:1:0:
d:\arduio doucument\libraries\Seeed_Arduino_LSM6DS3/LSM6DS3.h:32:18: error: expected declaration specifiers or '...' before numeric constant
 #define I2C_MODE 0
                  ^
D:\arduio doucument\hudie\IMU.c:5:15: note: in expansion of macro 'I2C_MODE'
D:\arduio doucument\hudie\IMU.c:5:25: error: expected declaration specifiers or '...' before numeric constant
D:\arduio doucument\hudie\IMU.c:8:0: warning: "M_PI" redefined
In file included from D:\arduio doucument\hudie\IMU.c:3:0:
c:\users\32494\appdata\local\arduino15\packages\seeeduino\tools\arm-none-eabi-gcc\7-2017q4\arm-none-eabi\include\math.h:617:0: note: this is the location of the previous definition
 #define M_PI  3.14159265358979323846
 
D:\arduio doucument\hudie\IMU.c: In function 'IMU_init':
D:\arduio doucument\hudie\IMU.c:23:4: error: 'myIMU' undeclared (first use in this function)
D:\arduio doucument\hudie\IMU.c:23:4: note: each undeclared identifier is reported only once for each function it appears in
D:\arduio doucument\hudie\IMU.c: In function 'bsp_IcmGetRawData':
D:\arduio doucument\hudie\IMU.c:116:24: error: 'myIMU' undeclared (first use in this function)
Using library Seeed Arduino LSM6DS3 at version 2.0.4 in folder: D:\arduio doucument\libraries\Seeed_Arduino_LSM6DS3 
Using library NRF52_TimerInterrupt at version 1.4.2 in folder: D:\arduio doucument\libraries\NRF52_TimerInterrupt 
Using library Adafruit TinyUSB Library at version 1.7.0 in folder: C:\Users\32494\AppData\Local\Arduino15\packages\Seeeduino\hardware\Seeed_nRF52_Boards\libraries\Adafruit_TinyUSB_Arduino 
Using library Wire at version 1.0 in folder: C:\Users\32494\AppData\Local\Arduino15\packages\Seeeduino\hardware\Seeed_nRF52_Boards\libraries\Wire 
Using library SPI at version 1.0 in folder: C:\Users\32494\AppData\Local\Arduino15\packages\Seeeduino\hardware\Seeed_nRF52_Boards\libraries\SPI 
exit status 1

Compilation error: exit status 1

And this is my code of IMU.c

#include <LSM6DS3.h>

#include <math.h>

LSM6DS3 myIMU(I2C_MODE, 0x6A);    //I2C device address 0x6A

extern uint32_t nowtime;
#define MM_PI  (float)3.1415926535
float TTangles_gyro[6]; //彤彤滤波角度
volatile float exInt, eyInt, ezInt;  // 误差积分
volatile float q0, q1, q2, q3; // 全局四元数
static double Gyro_fill[3][300];
static double Gyro_total[3];
static double sqrGyro_total[3];
static int GyroinitFlag = 0;
static int GyroCount = 0;
volatile uint32_t lastUpdate, now; // 采样周期计数 单位 us
int imu660ra_acc_x,imu660ra_acc_y,imu660ra_acc_z;
int imu660ra_gyro_x,imu660ra_gyro_y,imu660ra_gyro_z;

void IMU_init(void)
{
   myIMU.settings.gyroEnabled = 1;  //Can be 0 or 1
  myIMU.settings.gyroRange = 1000;   //Max deg/s.  Can be: 125, 245, 500, 1000, 2000
  myIMU.settings.gyroSampleRate = 833;   //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666
  myIMU.settings.gyroBandWidth = 200;  //Hz.  Can be: 50, 100, 200, 400;
  myIMU.settings.gyroFifoEnabled = 1;  //Set to include gyro in FIFO
  myIMU.settings.gyroFifoDecimation = 1;  //set 1 for on /1

  myIMU.settings.accelEnabled = 1;
  myIMU.settings.accelRange = 4;      //Max G force readable.  Can be: 2, 4, 8, 16
  myIMU.settings.accelSampleRate = 833;  //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
  myIMU.settings.accelBandWidth = 200;  //Hz.  Can be: 50, 100, 200, 400;
  myIMU.settings.accelFifoEnabled = 1;  //Set to include accelerometer in the FIFO
  myIMU.settings.accelFifoDecimation = 1;  //set 1 for on /1
  myIMU.settings.tempEnabled = 1;
  
  //Non-basic mode settings
  myIMU.settings.commMode = 1;


   myIMU.settings.fifoModeWord = 0;
  myIMU.begin();



  	q0 = 1.0f;  
		q1 = 0.0f;
		q2 = 0.0f;
		q3 = 0.0f;
		exInt = 0.0;
		eyInt = 0.0;
		ezInt = 0.0;
		lastUpdate = nowtime;
		now = nowtime;
}
// 方差变形公式 S^2 = (X1^2 + X2^2 + X3^2 + ... +Xn^2)/n - X平均^2
// 函数名称: calVariance
// 功能描述: 计算方差
// 输入参数: data[] --- 参与计算方差的数据缓冲区
//           length --- 数据长度
// 输出参数:                                                                                      */
//           sqrResult[] --- 方差结果
//           avgResult[] --- 平均数

void calGyroVariance(float data[], int length, float sqrResult[], float avgResult[])
{
	int i;
	double tmplen;
	if (GyroinitFlag == 0)
	{
		for (i = 0; i< 3; i++)
		{
			Gyro_fill[i][GyroCount] = data[i];
			Gyro_total[i] += data[i];
			sqrGyro_total[i] += data[i] * data[i];
			sqrResult[i] = 100;
			avgResult[i] = 0;
		}
	}
	else
	{
		for (i = 0; i< 3; i++)
		{
			Gyro_total[i] -= Gyro_fill[i][GyroCount];
			sqrGyro_total[i] -= Gyro_fill[i][GyroCount] * Gyro_fill[i][GyroCount];
			Gyro_fill[i][GyroCount] = data[i];
			Gyro_total[i] += Gyro_fill[i][GyroCount];
			sqrGyro_total[i] += Gyro_fill[i][GyroCount] * Gyro_fill[i][GyroCount];
		}
	}
	GyroCount++;
	if (GyroCount >= length)
	{
		GyroCount = 0;
		GyroinitFlag = 1;
	}
	if (GyroinitFlag == 0)
	{
		return;
	}
	tmplen = length;
	for (i = 0; i< 3; i++)
	{
		avgResult[i] = (float)(Gyro_total[i] / tmplen);
		sqrResult[i] = (float)((sqrGyro_total[i] - Gyro_total[i] * Gyro_total[i] / tmplen) / tmplen);
	}
}

int bsp_IcmGetRawData(float accel_mg[3], float gyro_dps[3])
{

	
      imu660ra_acc_x = myIMU.readRawAccelX();
      imu660ra_acc_y = myIMU.readRawAccelY();
      imu660ra_acc_z = myIMU.readRawAccelZ();      
      imu660ra_gyro_x = myIMU.readRawGyroX();
      imu660ra_gyro_y = myIMU.readRawGyroY();
      imu660ra_gyro_z = myIMU.readRawGyroZ();   
	accel_mg[0] = (float)(imu660ra_acc_x* 4 /* mg */) / 32.768;
	accel_mg[1] = (float)(imu660ra_acc_y* 4 /* mg */) / 32.768;
	accel_mg[2] = (float)(imu660ra_acc_z* 4 /* mg */) / 32.768;
	gyro_dps[0] = (float)(imu660ra_gyro_x* 1000 /* dps */) / 32768.0;
	gyro_dps[1] = (float)(imu660ra_gyro_y* 1000 /* dps */) / 32768.0;
	gyro_dps[2] = (float)(imu660ra_gyro_z* 1000 /* dps */) / 32768.0;
	
	return 0;
}
float my_gyro_offset[3] = {0};
int CalCount = 0;
/**************************实现函数********************************************
*函数原型:	   void IMU_getValues(float * values)
*功  能:	 读取加速度 陀螺仪 磁力计 的当前值  
输入参数： 将结果存放的数组首地址
输出参数：没有
*******************************************************************************/
void IMU_getValues(float * values) {  
	float accgyroval[6];
//	icm42688RealData_t accval;
//	icm42688RealData_t gyroval;
	
	float sqrResult_gyro[3];
	float avgResult_gyro[3];
	//读取加速度和陀螺仪的当前ADC
	bsp_IcmGetRawData(accgyroval, &accgyroval[3]);
    TTangles_gyro[0] =  accgyroval[0];
    TTangles_gyro[1] =  accgyroval[1];
    TTangles_gyro[2] =  accgyroval[2];
	TTangles_gyro[3] =  accgyroval[3];
	TTangles_gyro[4] =  accgyroval[4];
	TTangles_gyro[5] =  accgyroval[5];

	
	calGyroVariance(&TTangles_gyro[3], 100, sqrResult_gyro, avgResult_gyro);
	if (sqrResult_gyro[0] < 0.02f && sqrResult_gyro[1] < 0.02f && sqrResult_gyro[2] < 0.02f && CalCount >= 99)
	{
		my_gyro_offset[0] = avgResult_gyro[0];
		my_gyro_offset[1] = avgResult_gyro[1];
		my_gyro_offset[2] = avgResult_gyro[2];
		exInt = 0;
		eyInt = 0;
		ezInt = 0;
		CalCount = 0;
	}
	else if (CalCount < 100)
	{
		CalCount++;
	}
    values[0] =  accgyroval[0];
    values[1] =  accgyroval[1];
    values[2] =  accgyroval[2];
	values[3] =  accgyroval[3] - my_gyro_offset[0];
	values[4] =  accgyroval[4] - my_gyro_offset[1];
	values[5] =  accgyroval[5] - my_gyro_offset[2];
	

		//这里已经将量程改成了 1000度每秒  32.8 对应 1度每秒
}
// Fast inverse square-root
/**************************实现函数********************************************
*函数原型:	   float invSqrt(float x)
*功  能:	   快速计算 1/Sqrt(x) 	
输入参数： 要计算的值
输出参数： 结果
*******************************************************************************/
float invSqrt1(float x) {
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

/**************************实现函数********************************************
*函数原型:	   void IMU_AHRSupdate
*功  能:	 更新AHRS 更新四元数 
输入参数： 当前的测量值。
输出参数：没有
*******************************************************************************/
#define Kp 0.5f   // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.001f   // integral gain governs rate of convergence of gyroscope biases

void IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) {
  float norm;
  //float hx, hy, hz, bx, bz;
  float vx, vy, vz;//, wx, wy, wz;
  float ex, ey, ez,halfT;
  float tempq0,tempq1,tempq2,tempq3;

  // 先把这些用得到的值算好
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
  float q0q3 = q0*q3;
  float q1q1 = q1*q1;
  float q1q2 = q1*q2;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;   
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;   
////====================================================================================================================================
//	//20160323v0.4.6
//	//此处增加了一些机动检测使用的变量
//	//设置延迟启动机动检测的判据
//	static int s_InitTickCount=0;
//	float an[3]={0,0,0};
//	float Cb2n[3*3]={0};
//	

////====================================================================================================================================
//	
  
  now = nowtime;  //读取时间
  if(now<lastUpdate){ //定时器溢出过了。
  halfT =  ((float)(now + (0xffff- lastUpdate)) / 20000.0f);
  }
  else	{
  halfT =  ((float)(now - lastUpdate) / 20000.0f);
  }
  lastUpdate = now;	//更新时间
  
  norm = invSqrt1(ax*ax + ay*ay + az*az);       
  ax = ax * norm;
  ay = ay * norm;
  az = az * norm;
  //把加计的三维向量转成单位向量。

//  norm = invSqrt1(mx*mx + my*my + mz*mz);
//  mx = mx * norm;
//  my = my * norm;
//  mz = mz * norm;

  /*
  这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。
  */
  // compute reference direction of flux
//  hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
//  hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
//  hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);         
//  bx = sqrt((hx*hx) + (hy*hy));
//  bz = hz;     
  
  // estimated direction of gravity and flux (v and w)
  vx = 2*(q1q3 - q0q2);
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3;
  
//  /* 加速度：由南向北方向的加速度在加速度计X分量 */
//	north.x = 1 - 2*(q3*q3 + q2*q2);
//	/* 加速度：由南向北方向的加速度在加速度计Y分量 */
//	north.y = 2* (-q0*q3 + q1*q2);
//	/* 加速度：由南向北方向的加速度在加速度计Z分量 */
//	north.z = 2* (+q0*q2  - q1*q3);
//	/* 加速度：由东向西方向的加速度在加速度计X分量 */
//	west.x = 2* (+q0*q3 + q1*q2);
//	/* 加速度：由东向西方向的加速度在加速度计Y分量 */
//	west.y = 1 - 2*(q3*q3 + q1*q1);
//	/* 加速度：由东向西方向的加速度在加速度计Z分量 */
//	west.z = 2* (-q0*q1 + q2*q3);
//  wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
//  wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
//  wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
  
  // error is sum of cross product between reference direction of fields and direction measured by sensors
  ex = (ay*vz - az*vy);// + (my*wz - mz*wy);
  ey = (az*vx - ax*vz);// + (mz*wx - mx*wz);
  ez = (ax*vy - ay*vx);// + (mx*wy - my*wx);
  /*
  axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
  */
if(ex != 0.0f && ey != 0.0f && ez != 0.0f){
  exInt = exInt + ex * Ki * halfT;
  eyInt = eyInt + ey * Ki * halfT;	
  ezInt = ezInt + ez * Ki * halfT;

  // 用叉积误差来做PI修正陀螺零偏
  gx = gx + Kp*ex + exInt;
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;

  }

  // 四元数微分方程
  tempq0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  tempq1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  tempq2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  tempq3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
  
  // 四元数规范化
  norm = invSqrt1(tempq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);
  q0 = tempq0 * norm;
  q1 = tempq1 * norm;
  q2 = tempq2 * norm;
  q3 = tempq3 * norm;
}

/**************************实现函数********************************************
*函数原型:	   void IMU_getQ(float * q)
*功  能:	 更新四元数 返回当前的四元数组值
输入参数： 将要存放四元数的数组首地址
输出参数：没有
*******************************************************************************/
float mygetqval[6];	//用于存放传感器转换结果的数组
void IMU_getQ(float * q) {

  IMU_getValues(mygetqval);	 
  //将陀螺仪的测量值转成弧度每秒
  //加速度和磁力计保持 ADC值 不需要转换
 IMU_AHRSupdate(mygetqval[3] * MM_PI/180, mygetqval[4] * MM_PI/180, mygetqval[5] * MM_PI/180,
   mygetqval[0], mygetqval[1], mygetqval[2]);

  q[0] = q0; //返回当前值
  q[1] = q1;
  q[2] = q2;
  q[3] = q3;
}


/**************************实现函数********************************************
*函数原型:	   void IMU_getYawPitchRoll(float * angles)
*功  能:	 更新四元数 返回当前解算后的姿态数据
输入参数： 将要存放姿态角的数组首地址
输出参数：没有
*******************************************************************************/
void IMU_getYawPitchRoll(float * angles) {
  float q[4]; // 四元数
  volatile float gx=0.0, gy=0.0, gz=0.0; //估计重力方向
  IMU_getQ(q); //更新全局四元数
  
  angles[0] = -atan2(2 * q[1] * q[2] + 2 * q[0] * q[3], -2 * q[2]*q[2] - 2 * q[3] * q[3] + 1)* 180/MM_PI; // yaw
  angles[1] = -asin(-2 * q[1] * q[3] + 2 * q[0] * q[2])* 180/MM_PI; // pitch
  angles[2] = atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], -2 * q[1] * q[1] - 2 * q[2] * q[2] + 1)* 180/MM_PI; // roll
 // if(angles[0]<0)angles[0]+=360.0f;  //将 -+180度  转成0-360度
}

/*******************************************************************************************************************/
````预先格式化的文本`
And this is my code of IMU.h:


#ifndef IMU_NEW_H

#define IMU_NEW_H

extern float q0, q1, q2, q3; // 全局四元数

extern float exInt, eyInt, ezInt; // 误差积分

extern uint32_t lastUpdate, now; // 采样周期计数 单位 us

void IMU_getYawPitchRoll(float * angles);

void IMU_init(void);

#endif

Can you tell me why?