学习四旋翼（三）：DMP姿态解算和串级PID控制姿态

暑假期间，对于四旋翼有一点兴趣，没有亲手做，但是看了一些资料。这个系列文章只是对自己看的东西的记录，对于想要学习了解相关知识的同学没有任何参考价值！

本篇是系列第三篇，介绍了我对于MPU9250 DMP的使用，以及我对于如何运用测得的数据实现姿态控制的理解。

1.DMP的使用
DMP是陀螺仪传感器内置的姿态解算器，使用它能极大减少单片机的资源使用。可以通过移植公司提供的DMP代码库，调用，然后通过IIC读取数据。

我下载的开源代码包已经移植好了DMP代码库，但是在不同的平台上移植还是非常繁琐的。可以搜索小马哥的四轴课程中专门讲配置的那一堂课。

DMP的主要函数写在了inv_mpu.c之中，看了它的四元数转欧拉角的函数，和书上介绍的方法差不多。

void dmp_data_process()
{
	OS_ERR err;
static	float norm=0,qtemp[4],q[4];
static short gy[3],acc[3],sensors;
static u8 more;
static long quat[4];
	

	
	READ:	dmp_read_fifo(gy, acc, quat,0,&sensors, &more);
	if(more!=0)
{goto READ;}
	
					MPU_ANGLE.gx=(float)gy[0]/16.4;	MPU_ANGLE.gy=(float)gy[1]/16.4;	MPU_ANGLE.gz=(float)-gy[2]/16.4;


	qtemp[0] = (float)quat[0];
	qtemp[1] = (float)quat[1];
	qtemp[2] = (float)quat[2];
	qtemp[3] = (float)quat[3];
	norm = dmpinvSqrt(qtemp[0]*qtemp[0] + qtemp[1]*qtemp[1] + qtemp[2]*qtemp[2] + qtemp[3]*qtemp[3]);
	q[0] = qtemp[0] * norm;
	q[1] = qtemp[1] * norm;
	q[2] = qtemp[2] * norm;
	q[3] = qtemp[3] * norm;
	 MPU_ANGLE.Roll = (atan2(2.0*(q[0]*q[1] + q[2]*q[3]),
	                       1 - 2.0*(q[1]*q[1] + q[2]*q[2])))* 180/M_PI;
					
	 // we let safe_asin() handle the singularities near 90/-90 in pitch
MPU_ANGLE.Pitch = dmp_asin(2.0*(q[0]*q[2] - q[3]*q[1]))* 180/M_PI;
        
	MPU_ANGLE.Yaw = -atan2(2.0*(q[0]*q[3] + q[1]*q[2]),
	                     1 - 2.0*(q[2]*q[2] + q[3]*q[3]))* 180/M_PI;
}

当然使用者可以完全不管它，只要能读出数据就行。配置IIC初始化成功后，使用 MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
读出数据即可。

2.串级PID实现姿态控制
首先设计一个PID结构体

typedef struct
{
    float P;
    float I;
    float D;
    float Desired;
    float Error;
    float PreError;
    float PrePreError;
    float Integ;
    float Deriv;
    float Output;
 
}PID_Typedef;

编写一个通用的PID处理函数供调用

void PID_Postion_Cal(PID_Typedef * PID,float target,float measure,u16 thro_rc)
{


	PID->Error=target-measure;
	
if(thro_rc>=thro_start)	
{	PID->Integ+=PID->Error*dt;
		if(PID->Integ>Integ_max){PID->Integ=Integ_max;}
	  if(PID->Integ<-Integ_max){PID->Integ=-Integ_max;}
	}
else{PID->Integ=0;}

  PID->Deriv=PID->Error-PID->PreError;
	if(PID->Deriv>25){PID->Deriv=25;}
	if(PID->Deriv<-25){PID->Deriv=-25;}
	PID->Output=PID->P*PID->Error+PID->I*PID->Integ+PID->D*PID->Deriv;
	PID->PreError=PID->Error;
	
}

注意代码中对于积分和微分的最大值都进行了限制幅度。
该函数第一个参数用于结构体传参，第二个是目标值，第三个是当前的输入值。

void control(){
RC_ANGLE.X=roll;
RC_ANGLE.Y=pitch;
RC_ANGLE.Z=yaw;
	PID_Postion_Cal(&pitch_angle_PID,RC_CON.Y,MPU_ANGLE.Pitch,thro); 
	PID_Postion_Cal(&roll_angle_PID,RC_CON.X,MPU_ANGLE.Roll,thro);
//以上是外环的绕X,Y轴角度控制
  if(RC_CON.Z==0)
    {
        if(YawLockState == 0)
        {
         YawLock = MPU_ANGLE.Yaw;
					      
          YawLockState = 1;
        }
    }
    else 
		{
			YawLockState=0;
			YawLock = MPU_ANGLE.Yaw;
		} 
    PID_yaw(&yaw_angle_PID,YawLock,MPU_ANGLE.Yaw);  
    //以上是Z轴外环的角度控制   
	PID_yaw(&yaw_rate_PID,yaw_angle_PID.Output + RC_CON.Z,MPU_ANGLE.gz);
    PID_Postion_Cal(&pitch_rate_PID,pitch_angle_PID.Output,MPU_ANGLE.gy,thro);//  DMP½ÇËÙ¶È	
		
    PID_Postion_Cal(&roll_rate_PID,roll_angle_PID.Output,MPU_ANGLE.gx,thro);
//以上是三轴的内环控制                        

    Pitch = pitch_rate_PID.Output;
    Rool  = roll_rate_PID.Output;
    Yaw   = yaw_rate_PID.Output; 


	Motor[2] = (int16_t)(thro+thro_add+Pitch -Rool-Yaw );    //M3  
    Motor[0] = (int16_t)(thro+thro_add-Pitch +Rool -Yaw);    //M1
    Motor[3] = (int16_t)(thro+thro_add +Pitch +Rool +Yaw);    //M4 
    Motor[1] = (int16_t)( thro+thro_add-Pitch -Rool +Yaw);    //M2  thro 
if(thro<thro_start){Motor[0]=0;Motor[1]=0;Motor[2]=0;Motor[3]=0;}
    MotorPwmFlash(Motor[0],Motor[1],Motor[2],Motor[3]);  
}
}//将输出作为输入电调的参数

以上就是串级PID的控制代码，最终输出量是电调的值。