代码解析（自用自用自用怕之后搞忘球）

最新推荐文章于 2025-08-27 21:34:33 发布

1620°四周半地菜

最新推荐文章于 2025-08-27 21:34:33 发布

阅读量247

点赞数 11

CC 4.0 BY-SA版权

文章标签：学习

本文链接：https://blog.csdn.net/m0_74265954/article/details/140595177

1.mpu6050_data结构体

typedef struct {
	I2C_HandleTypeDef *i2cx;
	uint8_t port;
	Quaternion normalizedQuaternion;
	float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer
	int Offset[6];
	volatile float q0, q1, q2, q3;          // quaternion of sensor frame relative to auxiliary frame
}Mpu6050_data;

2.sensor_init（）

void sensor_init(Mpu6050_data *sensor, I2C_HandleTypeDef *i2cx, uint8_t port, int *offset)
{
	sensor->i2cx = i2cx;
	sensor->port = port;
	sensor->q0 = 1.0f;
	sensor->q1 = 0.0f;
	sensor->q2 = 0.0f;
	sensor->q3 = 0.0f;
	sensor->accelBias[0] = 0;
	sensor->accelBias[1] = 0;
	sensor->accelBias[2] = 0;
	sensor->gyroBias[0] = 0;
	sensor->gyroBias[1] = 0;
	sensor->gyroBias[2] = 0;
	sensor->normalizedQuaternion.w = 1.0;
	sensor->normalizedQuaternion.x = 0.0;
	sensor->normalizedQuaternion.y = 0.0;
	sensor->normalizedQuaternion.z = 0.0;	
	for(uint8_t i = 0; i < 3; i++)
		sensor->Offset[i+3] = offset[i+3];
	selectPort(i2cx, port);	
  // Self Test
  MPU6050SelfTest(sensor, SelfTest);

  // Initialize MPU6050
  MPU6050_Init(sensor);
}

函数作用：初始化传感器

应用举例：

sensor_init(&armR, &hi2c2, 1, offsetAcGyA);

每个参数的作用分别是：选择传感器、选择使用i2C几（与传感器绑定在哪个i2c有关）、pca9548片选（i2c拓展器，是一个模拟开关，适用于多个地址相同的外设）、选择对应的漂移校准参数

MPU6050SelfTest(sensor, SelfTest)函数用于自检

MPU6050_Init(sensor)函数用于初始化陀螺仪

3.sensor_update（）

void sensor_update(Mpu6050_data *sensor)
{
  selectPort(sensor->i2cx, sensor->port);
  // Get raw data
  mpu6050_GetData(sensor);

  // Update raw data to Quaternion form
  mpu6050_updateQuaternion(sensor);
/*
   Initially, sampleFreq should be set as (1000000.0f / (Now - lastUpdate)).
   However, when reading data from multiple sensors, this setting appears to be incorrect.
   In theory, this value should not be dependent on the number of sensors.
   Nevertheless, to ensure the proper functioning of the program, it is necessary to set it as
   (1000000.0f / (number of sensors) / (Now - lastUpdate)).
*/
  Now = micros();
  sampleFreq = (100000.0f / (Now - lastUpdate)); 
  lastUpdate = Now;

  //compute data
  MahonyAHRSupdateIMU(sensor, gxrs, gyrs, gzrs, axg, ayg, azg);
  correct_quaternion(sensor);
  //print("%.3f,%.3f,%.3f,%.3f;",sensor->q0,sensor->q1,sensor->q2,sensor->q3);
}

函数作用：更新四元数

4.init_quaternion（）

void init_quaternion(Mpu6050_data *sensor)
{
	Quaternion sensorQuat;
	sensorQuat.w = sensor->q0;
	sensorQuat.x = sensor->q1;
	sensorQuat.y = sensor->q2;
	sensorQuat.z = sensor->q3;
	quaternion_inverseTransformation(&sensorQuat);
	sensor->normalizedQuaternion.w = sensorQuat.w;
	sensor->normalizedQuaternion.x = sensorQuat.x;
	sensor->normalizedQuaternion.y = sensorQuat.y;
	sensor->normalizedQuaternion.z = sensorQuat.z;
}

函数作用：初始化并处理一个四元数

主要起作用的是这一个函数——quaternion_inverseTransformation(&sensorQuat);

子函数作用：对一个给定的四元数进行逆变换。四元数是一种用于表示三维空间中旋转的方法，它由一个实部和三个虚部组成。函数通过修改传入的四元数指针来实现这一点。

typedef struct {
    float w, x, y, z;
} Quaternion;

Quaternion结构体