基于ESP32四旋翼无人机的MPU6050姿态数据采集和处理方式

1. MPU6050姿态数据采集和处理

在DMP任务里面采集MPU6050的四元数的姿态数据，然后解算出机体坐标系的三个角姿态角。

dmp_task任务通过消息队列将姿态角发送给姿态控制程序
dmp_task->dandelion_onImu->
xQueueSend(mDandelion.dmp.poseQueue, &projection, 2)

程序如下：
static void dmp_task(void* arg)
{

// Initial MPU6050
mpl_i2c_probe(DMP_IIC_PORT);
mDmpHandler.stage = -1 ;
process_dmp_init_cfg();

// Get Gravity Bias
mDmpHandler.stage = -2 ;
process_dmp_init_bias();

// Flag Success
mDmpHandler.stage = 0 ;
int mpu6050_fifo_count = 0;

// Reset Filter
if(1)
{
	kalman1_init(&mDmpHandler.filter.kswPitch, 0.0, 0.0);
	mDmpHandler.filter.kswPitch.q = 0.01 ;
	mDmpHandler.filter.kswPitch.r = 0.02 ;
	mDmpHandler.filter.kswPitch.A = 0.96 ;

	kalman1_init(&mDmpHandler.filter.kswRoll, 0.0, 0.0);
	mDmpHandler.filter.kswRoll.q = 0.01 ;
	mDmpHandler.filter.kswRoll.r = 0.02 ;
	mDmpHandler.filter.kswRoll.A = 0.96 ;

	kalman1_init(&mDmpHandler.filter.kswYaw, 0.0, 0.0);
	mDmpHandler.filter.kswYaw.q = 0.01 ;
	mDmpHandler.filter.kswYaw.r = 0.02 ;
	mDmpHandler.filter.kswYaw.A = 0.96 ;
}

while(1)
{
	vTaskDelay(2);
	
	unsigned char more = 1;
	while(more)
	{

		// DMP 
		// 0x10000  -> +-2G / +-2000dps

		// Prepare Variables
		float pitch ,roll ,yaw ;
		short gyro[3], accel[3], sensors;
		long  quat[4];
		unsigned long sensor_timestamp;
		
		if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more)==0)
		{
			if(sensors&(INV_WXYZ_QUAT|INV_XYZ_ACCEL)) 
			{
				float q0,q1,q2,q3;
				
			
				q0 = quat[0] / q30; 
				q1 = quat[1] / q30; 
				q2 = quat[2] / q30; 
				q3 = quat[3] / q30; 

				// Compute Pose
				pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) ; // Unit = Rad
				roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1) ; // Unit = Rad
				yaw   = 0 ;

				float gravity   = (1.0 * (accel[2] - 0x4000)) / 0x4000;         // 
				float pitchGyro = ((1.0 * (gyro[1])) / 0x8000) * 2000 / 57.32; // Unit = Rad/s
				float rollGyro  = ((1.0 * (gyro[0])) / 0x8000) * 2000 / 57.32; // Unit = Rad/s
				float yawGyro   = ((1.0 * (gyro[2])) / 0x8000) * 2000 / 57.32; // Unit = Rad/s

				// Filter
				pitchGyro = kalman1_filter(&mDmpHandler.filter.kswPitch, pitchGyro);
				pitchGyro = kalman1_filter(&mDmpHandler.filter.kswRoll , pitchGyro);
				pitchGyro = kalman1_filter(&mDmpHandler.filter.kswYaw  , pitchGyro);

				mpu6050_fifo_count ++ ;
				mpu6050_fifo_count %= 200;
				if(mpu6050_fifo_count==0)
				{
					ESP_LOGI(TAG,"Get Pose pitch=% 1.2f|% 1.2f roll=% 1.2f|% 1.2f yaw=% 1.2f|% 1.2f",
						pitch,pitchGyro,roll,rollGyro,yaw,yawGyro);
				}

				dandelion_onImu(0, pitch, roll, yaw, pitchGyro, rollGyro, yawGyro);//发送dmp6050数据给飞行控制任务
				
			}
		}

	}

}

}

2. 姿态数据发送给飞行姿态控制

姿态控制程序接收到数据后，会利用PID算法分别计算控制四个螺旋桨的转速补偿量，从而控制飞行姿态。

loop_process_launch->xQueueReceive(mDandelion.dmp.poseQueue, &mDandelion.dmp.poseCurrent, 0)
loop_process_flight->xQueueReceive(mDandelion.dmp.poseQueue, &mDandelion.dmp.poseCurrent, 0)

具体程序见我上一篇文章。四旋翼无人机如何根据PID算法进行电机的PWM duty补偿：https://blog.csdn.net/katerdaisy/article/details/127332243

3.机体坐标系：

旋翼无人机 pitch yaw roll值分析：
俯仰角（pitch）：正半轴位于坐标原点的水平面之上（抬头）时，俯仰角为正，反之为负。
滚转角（roll）: 机体向右滚为正，反之为负。
偏航角（yaw）：机头右偏航为正，反之为负。