陀螺仪：MPU6050 *1
舵机：MG996R *2
主控芯片：STM32F103C8T6 *1

这部分代码使用TIM2定时器中断提供100Hz的控制频率，并且针对舵机位置控制延迟的问题，对舵机位置做了一个微分比例补偿控制，提高了响应速度。这部分代码在 main.c 里面。

// 全局变量声明
	float pitch,roll,yaw; 	//欧拉角	
	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//四元数
	float hy=0,hp=0;   			//参考系下云台角
	float by,bp,br;         //参考系下姿态角（也就是欧拉角）
	float bhy=0,bhp=0,bhr=0;//机体系下云台角
	float Kd1=10.0;					//舵机1补偿系数	
	float Kd2=10.0;					//舵机2补偿系数	

/********************************************/
//定时器2中断服务程序 周期：10ms  频率：100Hz
void TIM2_IRQHandler(void)   //TIM2中断
{
	static int t=0;
	u8 button;
	float bhylast=0, bhplast=0;	     //机体系下上一时刻的云台角
	float bhyincre=0, bhpincre=0;      //机体系下上一时刻的云台角增量
	float bhycomp = 0, bhrcomp = 0;                 //舵机转角度补偿量，与角速度有关
	if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)     //检查TIM2更新中断发生与否
	{	 TIM_ClearITPendingBit(TIM2, TIM_IT_Update  );    //清除更新中断标志 	
			
			bhylast=bhy, bhplast=bhp;		   			//记录机体系下上一时刻的云台角
			if(mpu_dmp_get_data(&roll,&pitch,&yaw)==0)   //更新姿态角(由于安装问题实际roll和pitch互换了)
			{ 
				yaw=-yaw, pitch=-pitch, roll=-roll;     //由于安装问题实际yaw,roll,pitch均倒向			
				
				CalHeadDegree();               //利用欧拉角旋转矩阵计算机体系下的云台角:hby,hbp
				
				bhyincre = bhy - bhylast;      //计算云台转动角速度
				bhpincre = bhp - bhplast;    
						
				bhycomp = Kd1*bhyincre;         //计算舵机1补偿量
				bhrcomp = Kd2*bhpincre;  		//计算舵机2补偿量
				
				HeadTurnToXY(bhy+bhycomp ,bhp+bhrcomp);    //进行角度补偿输出
						
				t++;
			  if(t>=10)t=0,	LED0=!LED0;  //每10*10=100ms闪动	
			}	
	}
}

这个函数将云台在参考坐标系内的指向向量旋转到机体系去，并且化为机体系下的云台角，注释掉的部分是用四元数表示的旋转矩阵，此部分放在 servo.c 里面。

/********************************************/
void CalHeadDegree(void)      //计算机体系下的云台角
{
	const float DEG2RAD = 0.0174533;   //度化弧度因子
	const float RAD2DEG = 57.29578 ;   //弧度化度因子
	float xn=cos(hy*DEG2RAD)*cos(hp*DEG2RAD);   //参考系下的云台指向向量
	float	yn=sin(hy*DEG2RAD)*cos(hp*DEG2RAD);
	float zn=sin(hp*DEG2RAD);           
	float xb,yb,zb,normalizer;  			//机体系下的云台指向向量、模长
	float cby,sby,cbp,sbp,cbr,sbr;       //姿态角的正余弦值
//	printf("xn:%f\tyn:%f\tzn:%f\r\n",xn,yn,zn);

	normalizer = invSqrt(xn*xn + yn*yn +zn*zn);   //求出向量模长的倒数		
	xn *= normalizer;	        //向量单位化
	yn *= normalizer;
	zn *= normalizer;	
	
	bp=pitch,br=roll,by=yaw;				//先算出姿态角的三角值方便计算
	cby=cos(by*DEG2RAD),sby=sin(by*DEG2RAD);       
	cbp=cos(bp*DEG2RAD),sbp=sin(bp*DEG2RAD);
	cbr=cos(br*DEG2RAD),sbr=sin(br*DEG2RAD);
//  printf("yaw:%f\tpitch:%f\troll:%f\r\n",yaw,pitch,roll);
	
	xb =                 cbp*cby*xn                 + cbp*sby*yn      -sbp*zn;//套用欧拉角旋转矩阵公式
	yb = (sbr*sbp*cby - cbr*sby)*xn + (sbr*sbp*sby + cbr*cby)*yn + sbr*cbp*zn;
	zb = (cbr*sbp*cby + sbr*sby)*xn + (cbr*sbp*sby - sbr*cby)*yn + cbr*cbp*zn;
//  printf("xb:%f\tyb:%f\tzb:%f\t",xb,yb,zb);

//	xb = (q0*q0+q1*q1-q2*q2-q3*q3)*xn + 2*(q1*q2-q0*q3)*yn + 2*(q1*q3+q0*q2)*zn;//套用四元数旋转矩阵公式	               
//	yb = 2*(q1*q2+q0*q3)*xn +(q0*q0-q1*q1+q2*q2-q3*q3)*yn + 2*(q2*q3-q0*q1)*zn;   
//	zb = 2*(q1*q3-q0*q2)*xn + 2*(q2*q3+q0*q1) *yn + (q0*q0-q1*q1-q2*q2+q3*q3)*zn;	
//	xb=yb, yb=-xb;
	
	normalizer = invSqrt(xb*xb + yb*yb +zb*zb);   //求出向量模长的倒数		
	xb *= normalizer;	        //向量单位化
	yb *= normalizer;
	zb *= normalizer;	
//  printf("normalizer:%f\r\n",normalizer);
	                                                    //利用几何关系计算机体系下的云台角 
	bhp = asin(zb)*RAD2DEG;          										
	bhy = acos( xb*invSqrt(xb*xb+yb*yb) )*RAD2DEG;		
	bhp = -bhp;									         //加上正负号
	if(yb<0) bhy=-bhy;           
//	printf("bhp:%f\tbhy:%f\r\n",bhp,bhy);
}

以下函数用来控制舵机，使它们指向机体系下的一个方向 (hby,hbp)
水平角hby:-90°~90° 俯仰角hbp: 0°~180° ，放在 servo.c 里面。

/********************************************/
/************************************************/
void Servo1RunToDegree(float degree)   //舵机1转到degree角度,degree:-90~90
{
	float pwm;
	if(degree>=-120 && degree<=120){
		pwm = 1.0394*degree + 1848.55;
		TIM_SetCompare2(TIM3,pwm);	//修改比较值，修改占空比
	}
}
void Servo2RunToDegree(float degree)   //舵机2转到degree角度,degree:0~180
{
	float pwm;
	if(degree>=-30 && degree<=210){
		pwm = -1.0306*degree + 1939.10; 
		TIM_SetCompare1(TIM4,pwm);	//修改比较值，修改占空比	
	}
}

/************************************************/
void HeadTurnToXY(float x,float y)       //云台转到水平x,俯仰y位置, x:-90~90, y:0~180
{
		Servo1RunToDegree(x);
	  Servo2RunToDegree(y);
}