在好例子网，分享、交流、成长！
您当前所在位置：首页 → C/C++ 开发实例 → 嵌入式开发 → stm32四轴飞行器mpu6050+飞控

stm32四轴飞行器mpu6050+飞控

嵌入式开发

下载此实例
  • 开发语言：C/C++
  • 实例大小：0.02M
  • 下载次数：38
  • 浏览次数：527
  • 发布时间：2016-03-03
  • 实例类别：嵌入式开发
  • 发 布 人：access
  • 文件格式：.c
  • 所需积分：2
 相关标签： STM32 2
网友评论 举报投诉 收藏该页

实例介绍

【实例简介】
【实例截图】
【核心代码】/******************** (C) COPYRIGHT 2014 ANO Tech ********************************
/******************** (C) COPYRIGHT 2014 ANO Tech ********************************
  * ×÷Õß   £ºÄäÃû¿Æ´´
 * ÎļþÃû  £ºdata_transfer.c
 * ÃèÊö    £ºÊý¾Ý´«Êä
 * ¹ÙÍø    £ºwww.anotc.com
 * ÌÔ±¦    £ºanotc.taobao.com
 * ¼¼ÊõQȺ £º190169595
**********************************************************************************/

//#include "data_transfer.h"
#include "usart.h"
//#include "imu.h"
#include "mpu6050.h"
//#include "ak8975.h"
//#include "ms5611.h"
//#include "rc.h"
//#include "ctrl.h"
#include "time.h"
//#include "usbd_user_hid.h"
#include "ANO_DT.h"
#include "stm32f10x.h"
#include "v2.6.h"
#include "mpu6050.h"
 
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" 

/////////////////////////////////////////////////////////////////////////////////////
//Êý¾Ý²ð·Öºê¶¨Ò壬ÔÚ·¢ËÍ´óÓÚ1×Ö½ÚµÄÊý¾ÝÀàÐÍʱ£¬±ÈÈçint16¡¢floatµÈ£¬ÐèÒª°ÑÊý¾Ý²ð·Ö³Éµ¥¶À×Ö½Ú½øÐз¢ËÍ
#define BYTE0(dwTemp)       ( *( (char *)(&dwTemp)		) )
#define BYTE1(dwTemp)       ( *( (char *)(&dwTemp)   1) )
#define BYTE2(dwTemp)       ( *( (char *)(&dwTemp)   2) )
#define BYTE3(dwTemp)       ( *( (char *)(&dwTemp)   3) )
	
	float pit,rol,ya; 		//Å·À­½Ç
  u8 pitch,roll,yaw; 		//Å·À­½Ç
	u8 aacx,aacy,aacz;		//¼ÓËٶȴ«¸ÐÆ÷ԭʼÊý¾Ý
	u8 gyrox,gyroy,gyroz;	//ÍÓÂÝÒÇԭʼÊý¾Ý
short acx,acy,acz;		//¼ÓËٶȴ«¸ÐÆ÷ԭʼÊý¾Ý
	short grox,groy,groz;	//ÍÓÂÝÒÇԭʼÊý¾Ý

float rollkp=800,rollki=500,rollkd=30,pitchkp=500,pitchki=200,pitchkd=35,yawkp=700,yawki=30,yawkd=40;
dt_flag_t f;					//ÐèÒª·¢ËÍÊý¾ÝµÄ±êÖ¾
u8 data_to_send[50];	//·¢ËÍÊý¾Ý»º´æ

/////////////////////////////////////////////////////////////////////////////////////
void ANO_DT_Send_Data(u8*data_to_send, u8 _cnt)
{ int i;
  for(i=0;i<_cnt;i  ) usart1_send_char(data_to_send[i]);


}
//Data_Exchangeº¯Êý´¦Àí¸÷ÖÖÊý¾Ý·¢ËÍÇëÇ󣬱ÈÈçÏëʵÏÖÿ5ms·¢ËÍÒ»´Î´«¸ÐÆ÷Êý¾ÝÖÁÉÏλ»ú£¬¼´Ôڴ˺¯ÊýÄÚʵÏÖ
//´Ëº¯ÊýÓ¦ÓÉÓû§Ã¿1msµ÷ÓÃÒ»´Î
void ANO_DT_Data_Exchange(void)
{
	static u8 cnt = 0;
	static u8 senser_cnt 	= 10;
	static u8 status_cnt 	= 15;
	static u8 rcdata_cnt 	= 20;
	static u8 motopwm_cnt	= 20;
	static u8 power_cnt		=	50;
	MPU_Get_Accelerometer(&acx,&acy,&acz);	//µÃµ½¼ÓËٶȴ«¸ÐÆ÷Êý¾Ý
			MPU_Get_Gyroscope(&grox,&groy,&groz);	//µÃµ½ÍÓÂÝÒÇÊý¾Ý
	mpu_dmp_get_data(&pit,&rol,&ya);
	if((cnt % senser_cnt) == (senser_cnt-1))
		f.send_senser = 1;	
	
	if((cnt % status_cnt) == (status_cnt-1))
		f.send_status = 1;	
	
	if((cnt % rcdata_cnt) == (rcdata_cnt-1))
		f.send_rcdata = 1;	
	
	if((cnt % motopwm_cnt) == (motopwm_cnt-1))
		f.send_motopwm = 1;	
	
	if((cnt % power_cnt) == (power_cnt-1))
		f.send_power = 1;		
	
	cnt  ;
/////////////////////////////////////////////////////////////////////////////////////
	if(f.send_version)  //°æ±¾ÐÅÏ¢ 
	{
		f.send_version = 0;
		ANO_DT_Send_Version(4,300,100,400,0);
	}
/////////////////////////////////////////////////////////////////////////////////////
	else if(f.send_status)  //pit roll yaw ¸ß¶È  ·ÉÐÐģʽ  0½âËø 1¼ÓËø
	{
		f.send_status = 0;
    roll=rol;
		pitch=pit;
		yaw=ya;
		ANO_DT_Send_Status(rol,pit,ya,500,1,1);//¸ßµÄcm
	}	
/////////////////////////////////////////////////////////////////////////////////////
	else if(f.send_senser)
	{
		f.send_senser = 0;
		aacx=acx;
		aacy=acy;
		aacz=acz;
		gyrox=grox;
		gyroy=groy;
		gyroz=groz;
		ANO_DT_Send_Senser(aacx,aacy,aacz,
												gyrox,gyroy,gyroz,
												0,0,0,0);
	}	
/////////////////////////////////////////////////////////////////////////////////////
//	else if(f.send_rcdata)
//	{
//		f.send_rcdata = 0;
//		ANO_DT_Send_RCData(Rc_Pwm_In[0],Rc_Pwm_In[1],Rc_Pwm_In[2],Rc_Pwm_In[3],Rc_Pwm_In[4],Rc_Pwm_In[5],Rc_Pwm_In[6],Rc_Pwm_In[7],0,0);
//	}	
/////////////////////////////////////////////////////////////////////////////////////	
	else if(f.send_motopwm)
	{
		f.send_motopwm = 0;
		ANO_DT_Send_MotoPWM(900,2,5,10000,8,6,6,8);
	}	
/////////////////////////////////////////////////////////////////////////////////////
	else if(f.send_power)
	{
		f.send_power = 0;
		ANO_DT_Send_Power(234,456); //µçѹ µçÁ÷
	}
/////////////////////////////////////////////////////////////////////////////////////
	else if(f.send_pid1)
	{
		f.send_pid1 = 0;
		ANO_DT_Send_PID(1,rollkp,rollki,rollkd,pitchkp,pitchki,pitchkd,yawkp,yawki,yawkd);
	}	
/////////////////////////////////////////////////////////////////////////////////////
	else if(f.send_pid2)
	{
		f.send_pid2 = 0;
		//ANO_DT_Send_PID(2,p1_p,p1_i,p1_d,p2_p,p2_i,p2_d,p3_p,p3_i,p3_d);
	}
/////////////////////////////////////////////////////////////////////////////////////
	else if(f.send_pid3)
	{
		f.send_pid3 = 0;
		//ANO_DT_Send_PID(3,p1_p,p1_i,p1_d,p2_p,p2_i,p2_d,p3_p,p3_i,p3_d);
	}
///////////////////////////////////////////////////////////////////////////////////////

}

///////////////////////////////////////////////////////////////////////////////////////
////Send_Dataº¯ÊýÊÇЭÒéÖÐËùÓз¢ËÍÊý¾Ý¹¦ÄÜʹÓõ½µÄ·¢Ëͺ¯Êý
////ÒÆֲʱ£¬Óû§Ó¦¸ù¾Ý×ÔÉíÓ¦ÓõÄÇé¿ö£¬¸ù¾ÝʹÓõÄͨÐÅ·½Ê½£¬ÊµÏִ˺¯Êý
//void ANO_DT_Send_Data(u8 *dataToSend , u8 length)
//{
//#ifdef ANO_DT_USE_USB_HID
//	Usb_Hid_Adddata(data_to_send,length);
//#endif
//#ifdef ANO_DT_USE_USART2
//	Usart2_Send(data_to_send, length);
//#endif
//}

static void ANO_DT_Send_Check(u8 head, u8 check_sum)
{  u8 sum = 0,i;
	data_to_send[0]=0xAA;
	data_to_send[1]=0xAA;
	data_to_send[2]=0xEF;
	data_to_send[3]=2;
	data_to_send[4]=head;
	data_to_send[5]=check_sum;
	
	
	
	for(i=0;i<6;i  )
		sum  = data_to_send[i];
	data_to_send[6]=sum;

	ANO_DT_Send_Data(data_to_send, 7);
}

/////////////////////////////////////////////////////////////////////////////////////
//Data_Receive_Prepareº¯ÊýÊÇЭÒéÔ¤½âÎö£¬¸ù¾ÝЭÒéµÄ¸ñʽ£¬½«ÊÕµ½µÄÊý¾Ý½øÐÐÒ»´Î¸ñʽÐÔ½âÎö£¬¸ñʽÕýÈ·µÄ»°ÔÙ½øÐÐÊý¾Ý½âÎö
//ÒÆֲʱ£¬´Ëº¯ÊýÓ¦ÓÉÓû§¸ù¾Ý×ÔÉíʹÓõÄͨÐÅ·½Ê½×ÔÐе÷Ó㬱ÈÈç´®¿ÚÿÊÕµ½Ò»×Ö½ÚÊý¾Ý£¬Ôòµ÷Óô˺¯ÊýÒ»´Î
//´Ëº¯Êý½âÎö³ö·ûºÏ¸ñʽµÄÊý¾ÝÖ¡ºó£¬»á×ÔÐе÷ÓÃÊý¾Ý½âÎöº¯Êý
void ANO_DT_Data_Receive_Prepare(u8 data)
{
	static u8 RxBuffer[50];
	static u8 _data_len = 0,_data_cnt = 0;
	static u8 state = 0;
	
	if(state==0&&data==0xAA)
	{
		state=1;
		RxBuffer[0]=data;
	}
	else if(state==1&&data==0xAF)
	{
		state=2;
		RxBuffer[1]=data;
	}
	else if(state==2&&data<0XF1)
	{
		state=3;
		RxBuffer[2]=data;
	}
	else if(state==3&&data<50)
	{
		state = 4;
		RxBuffer[3]=data;
		_data_len = data;
		_data_cnt = 0;
	}
	else if(state==4&&_data_len>0)
	{
		_data_len--;
		RxBuffer[4 _data_cnt  ]=data;
		if(_data_len==0)
			state = 5;
	}
	else if(state==5)
	{
		state = 0;
		RxBuffer[4 _data_cnt]=data;
		ANO_DT_Data_Receive_Anl(RxBuffer,_data_cnt 5);
	}
	else
		state = 0;
}
/////////////////////////////////////////////////////////////////////////////////////
//Data_Receive_Anlº¯ÊýÊÇЭÒéÊý¾Ý½âÎöº¯Êý£¬º¯Êý²ÎÊýÊÇ·ûºÏЭÒé¸ñʽµÄÒ»¸öÊý¾ÝÖ¡£¬¸Ãº¯Êý»áÊ×ÏȶÔЭÒéÊý¾Ý½øÐÐУÑé
//УÑéͨ¹ýºó¶ÔÊý¾Ý½øÐнâÎö£¬ÊµÏÖÏàÓ¦¹¦ÄÜ
//´Ëº¯Êý¿ÉÒÔ²»ÓÃÓû§×ÔÐе÷Óã¬Óɺ¯ÊýData_Receive_Prepare×Ô¶¯µ÷ÓÃ
void ANO_DT_Data_Receive_Anl(u8 *data_buf,u8 num)
{
	u8 sum = 0,i;
	for(i=0;i<(num-1);i  )
		sum  = *(data_buf i);
	if(!(sum==*(data_buf num-1)))		return;		//ÅжÏsum
	if(!(*(data_buf)==0xAA && *(data_buf 1)==0xAF))		return;		//ÅжÏÖ¡Í·
	
//	if(*(data_buf 2)==0X01)               //mpu6050µ÷Áã
//	{
//		if(*(data_buf 4)==0X01)
//			mpu6050.Acc_CALIBRATE = 1;
//		if(*(data_buf 4)==0X02)
//			mpu6050.Gyro_CALIBRATE = 1;
//		if(*(data_buf 4)==0X03)
//		{
//			mpu6050.Acc_CALIBRATE = 1;		
//			mpu6050.Gyro_CALIBRATE = 1;			
//		}
//	}
//	
	if(*(data_buf 2)==0X02)     //¶ÁÈ¡pid
	{
		if(*(data_buf 4)==0X01)
		{
			f.send_pid1 = 1;
			f.send_pid2 = 1;
			f.send_pid3 = 1;
			f.send_pid4 = 1;
			f.send_pid5 = 1;
			f.send_pid6 = 1;
		}
		if(*(data_buf 4)==0X02)
		{
			
		}
		if(*(data_buf 4)==0XA0)		//¶ÁÈ¡°æ±¾ÐÅÏ¢
		{
			f.send_version = 1;
		}
		if(*(data_buf 4)==0XA1)		//»Ö¸´Ä¬ÈϲÎÊý
		{
			//Para_ResetToFactorySetup();
		}
	}

	if(*(data_buf 2)==0X10)								//PID1
    {
        rollkp  = 0.001*( (vs16)(*(data_buf 4)<<8)|*(data_buf 5) );
        rollki  = 0.001*( (vs16)(*(data_buf 6)<<8)|*(data_buf 7) );
        rollkd  = 0.001*( (vs16)(*(data_buf 8)<<8)|*(data_buf 9) );
        pitchkp = 0.001*( (vs16)(*(data_buf 10)<<8)|*(data_buf 11) );
        pitchki = 0.001*( (vs16)(*(data_buf 12)<<8)|*(data_buf 13) );
        pitchkd = 0.001*( (vs16)(*(data_buf 14)<<8)|*(data_buf 15) );
        yawkp 	= 0.001*( (vs16)(*(data_buf 16)<<8)|*(data_buf 17) );
        yawki 	= 0.001*( (vs16)(*(data_buf 18)<<8)|*(data_buf 19) );
        yawkd 	= 0.001*( (vs16)(*(data_buf 20)<<8)|*(data_buf 21) );
        ANO_DT_Send_Check(*(data_buf 2),sum);
				//Param_SavePID();
    }
    if(*(data_buf 2)==0X11)								//PID2
    {
//        ctrl_1.PID[PID4].kp 	= 0.001*( (vs16)(*(data_buf 4)<<8)|*(data_buf 5) );
//        ctrl_1.PID[PID4].ki 	= 0.001*( (vs16)(*(data_buf 6)<<8)|*(data_buf 7) );
//        ctrl_1.PID[PID4].kd 	= 0.001*( (vs16)(*(data_buf 8)<<8)|*(data_buf 9) );
//        ctrl_1.PID[PID5].kp 	= 0.001*( (vs16)(*(data_buf 10)<<8)|*(data_buf 11) );
//        ctrl_1.PID[PID5].ki 	= 0.001*( (vs16)(*(data_buf 12)<<8)|*(data_buf 13) );
//        ctrl_1.PID[PID5].kd 	= 0.001*( (vs16)(*(data_buf 14)<<8)|*(data_buf 15) );
//        ctrl_1.PID[PID6].kp	  = 0.001*( (vs16)(*(data_buf 16)<<8)|*(data_buf 17) );
//        ctrl_1.PID[PID6].ki 	= 0.001*( (vs16)(*(data_buf 18)<<8)|*(data_buf 19) );
//        ctrl_1.PID[PID6].kd 	= 0.001*( (vs16)(*(data_buf 20)<<8)|*(data_buf 21) );
        ANO_DT_Send_Check(*(data_buf 2),sum);
				//Param_SavePID();
    }
    if(*(data_buf 2)==0X12)								//PID3
    {	
//        ctrl_2.PID[PIDROLL].kp  = 0.001*( (vs16)(*(data_buf 4)<<8)|*(data_buf 5) );
//        ctrl_2.PID[PIDROLL].ki  = 0.001*( (vs16)(*(data_buf 6)<<8)|*(data_buf 7) );
//        ctrl_2.PID[PIDROLL].kd  = 0.001*( (vs16)(*(data_buf 8)<<8)|*(data_buf 9) );
//        ctrl_2.PID[PIDPITCH].kp = 0.001*( (vs16)(*(data_buf 10)<<8)|*(data_buf 11) );
//        ctrl_2.PID[PIDPITCH].ki = 0.001*( (vs16)(*(data_buf 12)<<8)|*(data_buf 13) );
//        ctrl_2.PID[PIDPITCH].kd = 0.001*( (vs16)(*(data_buf 14)<<8)|*(data_buf 15) );
//        ctrl_2.PID[PIDYAW].kp 	= 0.001*( (vs16)(*(data_buf 16)<<8)|*(data_buf 17) );
//        ctrl_2.PID[PIDYAW].ki 	= 0.001*( (vs16)(*(data_buf 18)<<8)|*(data_buf 19) );
//        ctrl_2.PID[PIDYAW].kd 	= 0.001*( (vs16)(*(data_buf 20)<<8)|*(data_buf 21) );
        ANO_DT_Send_Check(*(data_buf 2),sum);
				//Param_SavePID();
    }
	if(*(data_buf 2)==0X13)								//PID4
	{
		ANO_DT_Send_Check(*(data_buf 2),sum);
	}
	if(*(data_buf 2)==0X14)								//PID5
	{
		ANO_DT_Send_Check(*(data_buf 2),sum);
	}
	if(*(data_buf 2)==0X15)								//PID6
	{
		ANO_DT_Send_Check(*(data_buf 2),sum);
	}


}

void ANO_DT_Send_Version(u8 hardware_type, u16 hardware_ver,u16 software_ver,u16 protocol_ver,u16 bootloader_ver)
{
	u8 _cnt=0,i,sum = 0;
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0x00;
	data_to_send[_cnt  ]=0;
	
	data_to_send[_cnt  ]=hardware_type;
	data_to_send[_cnt  ]=BYTE1(hardware_ver);
	data_to_send[_cnt  ]=BYTE0(hardware_ver);
	data_to_send[_cnt  ]=BYTE1(software_ver);
	data_to_send[_cnt  ]=BYTE0(software_ver);
	data_to_send[_cnt  ]=BYTE1(protocol_ver);
	data_to_send[_cnt  ]=BYTE0(protocol_ver);
	data_to_send[_cnt  ]=BYTE1(bootloader_ver);
	data_to_send[_cnt  ]=BYTE0(bootloader_ver);
	
	data_to_send[3] = _cnt-4;
	
	
	for(i=0;i<_cnt;i  )
		sum  = data_to_send[i];
	data_to_send[_cnt  ]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Status(float angle_rol, float angle_pit, float angle_yaw, s32 alt, u8 fly_model, u8 armed)
{
	u8 _cnt=0;
	vs16 _temp;
	vs32 _temp2 = alt;
	int i;
	int sum = 0;
	
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0x01;
	data_to_send[_cnt  ]=0;
	
	_temp = (int)(angle_rol*100);
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = (int)(angle_pit*100);
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = (int)(angle_yaw*100);
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	
	data_to_send[_cnt  ]=BYTE3(_temp2);
	data_to_send[_cnt  ]=BYTE2(_temp2);
	data_to_send[_cnt  ]=BYTE1(_temp2);
	data_to_send[_cnt  ]=BYTE0(_temp2);
	
	data_to_send[_cnt  ] = fly_model;
	
	data_to_send[_cnt  ] = armed;
	
	data_to_send[3] = _cnt-4;
	
//  int sum = 0;
	for(i=0;i<_cnt;i  )
		sum  = data_to_send[i];
	data_to_send[_cnt  ]=sum;
	
	//for(i=0;i<_cnt;i  ) usart1_send_char(data_to_send[i]);
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Senser(s16 a_x,s16 a_y,s16 a_z,s16 g_x,s16 g_y,s16 g_z,s16 m_x,s16 m_y,s16 m_z,s32 bar)
{
	u8 _cnt=0,sum = 0,i;
	vs16 _temp;
	
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0x02;
	data_to_send[_cnt  ]=0;
	
	_temp = a_x;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = a_y;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = a_z;	
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	
	_temp = g_x;	
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = g_y;	
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = g_z;	
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	
	_temp = m_x;	
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = m_y;	
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = m_z;	
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	
	data_to_send[3] = _cnt-4;
	
	 
	for(i=0;i<_cnt;i  )
		sum  = data_to_send[i];
	data_to_send[_cnt  ] = sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_RCData(u16 thr,u16 yaw,u16 rol,u16 pit,u16 aux1,u16 aux2,u16 aux3,u16 aux4,u16 aux5,u16 aux6)
{
	u8 _cnt=0,i,sum = 0;
	
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0x03;
	data_to_send[_cnt  ]=0;
	data_to_send[_cnt  ]=BYTE1(thr);
	data_to_send[_cnt  ]=BYTE0(thr);
	data_to_send[_cnt  ]=BYTE1(yaw);
	data_to_send[_cnt  ]=BYTE0(yaw);
	data_to_send[_cnt  ]=BYTE1(rol);
	data_to_send[_cnt  ]=BYTE0(rol);
	data_to_send[_cnt  ]=BYTE1(pit);
	data_to_send[_cnt  ]=BYTE0(pit);
	data_to_send[_cnt  ]=BYTE1(aux1);
	data_to_send[_cnt  ]=BYTE0(aux1);
	data_to_send[_cnt  ]=BYTE1(aux2);
	data_to_send[_cnt  ]=BYTE0(aux2);
	data_to_send[_cnt  ]=BYTE1(aux3);
	data_to_send[_cnt  ]=BYTE0(aux3);
	data_to_send[_cnt  ]=BYTE1(aux4);
	data_to_send[_cnt  ]=BYTE0(aux4);
	data_to_send[_cnt  ]=BYTE1(aux5);
	data_to_send[_cnt  ]=BYTE0(aux5);
	data_to_send[_cnt  ]=BYTE1(aux6);
	data_to_send[_cnt  ]=BYTE0(aux6);

	data_to_send[3] = _cnt-4;
	
	
	for(i=0;i<_cnt;i  )
		sum  = data_to_send[i];
	
	data_to_send[_cnt  ]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Power(u16 votage, u16 current)
{
	u8 _cnt=0,i,sum=0;
	u16 temp;
	
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0x05;
	data_to_send[_cnt  ]=0;
	
	temp = votage;
	data_to_send[_cnt  ]=BYTE1(temp);
	data_to_send[_cnt  ]=BYTE0(temp);
	temp = current;
	data_to_send[_cnt  ]=BYTE1(temp);
	data_to_send[_cnt  ]=BYTE0(temp);
	
	data_to_send[3] = _cnt-4;
	
	
	for(i=0;i<_cnt;i  )
		sum  = data_to_send[i];
	
	data_to_send[_cnt  ]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_MotoPWM(u16 m_1,u16 m_2,u16 m_3,u16 m_4,u16 m_5,u16 m_6,u16 m_7,u16 m_8)
{
	u8 _cnt=0,i,sum = 0;
	
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0x06;
	data_to_send[_cnt  ]=0;
	
	data_to_send[_cnt  ]=BYTE1(m_1);
	data_to_send[_cnt  ]=BYTE0(m_1);
	data_to_send[_cnt  ]=BYTE1(m_2);
	data_to_send[_cnt  ]=BYTE0(m_2);
	data_to_send[_cnt  ]=BYTE1(m_3);
	data_to_send[_cnt  ]=BYTE0(m_3);
	data_to_send[_cnt  ]=BYTE1(m_4);
	data_to_send[_cnt  ]=BYTE0(m_4);
	data_to_send[_cnt  ]=BYTE1(m_5);
	data_to_send[_cnt  ]=BYTE0(m_5);
	data_to_send[_cnt  ]=BYTE1(m_6);
	data_to_send[_cnt  ]=BYTE0(m_6);
	data_to_send[_cnt  ]=BYTE1(m_7);
	data_to_send[_cnt  ]=BYTE0(m_7);
	data_to_send[_cnt  ]=BYTE1(m_8);
	data_to_send[_cnt  ]=BYTE0(m_8);
	
	data_to_send[3] = _cnt-4;
	
	
	for(i=0;i<_cnt;i  )
		sum  = data_to_send[i];
	
	data_to_send[_cnt  ]=sum;
	
	ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_PID(u8 group,float p1_p,float p1_i,float p1_d,float p2_p,float p2_i,float p2_d,float p3_p,float p3_i,float p3_d)
{
	u8 _cnt=0,i,sum = 0;
	vs16 _temp;
	
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0xAA;
	data_to_send[_cnt  ]=0x10 group-1;
	data_to_send[_cnt  ]=0;
	
	
	_temp = p1_p ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = p1_i  ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = p1_d  ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = p2_p  ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = p2_i  ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = p2_d ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = p3_p ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = p3_i ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	_temp = p3_d ;
	data_to_send[_cnt  ]=BYTE1(_temp);
	data_to_send[_cnt  ]=BYTE0(_temp);
	
	data_to_send[3] = _cnt-4;
	
	
	for(i=0;i<_cnt;i  )
		sum  = data_to_send[i];
	
	data_to_send[_cnt  ]=sum;

	ANO_DT_Send_Data(data_to_send, _cnt);
}

/******************* (C) COPYRIGHT 2014 ANO TECH *****END OF FILE************/


**********************************************************************************/

//#include "data_transfer.h"
#include "usart.h"
//#include "imu.h"
#include "mpu6050.h"
//#include "ak8975.h"
//#include "ms5611.h"
//#include "rc.h"
//#include "ctrl.h"
#include "time.h"
//#include "usbd_user_hid.h"
#include "ANO_DT.h"
#include "stm32f10x.h"
#include "v2.6.h"
#include "mpu6050.h"
 
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" 

/////////////////////////////////////////////////////////////////////////////////////
//Êý¾Ý²ð·Öºê¶¨Ò壬ÔÚ·¢ËÍ´óÓÚ1×Ö½ÚµÄÊý¾ÝÀàÐÍʱ£¬±ÈÈçint16¡¢floatµÈ£¬ÐèÒª°ÑÊý¾Ý²ð·Ö³Éµ¥¶À×Ö½Ú½øÐз¢ËÍ
#define BYTE0(dwTemp)       ( *( (char *)(&dwTemp) ) )
#define BYTE1(dwTemp)       ( *( (char *)(&dwTemp) 1) )
#define BYTE2(dwTemp)       ( *( (char *)(&dwTemp) 2) )
#define BYTE3(dwTemp)       ( *( (char *)(&dwTemp) 3) )

float pit,rol,ya; //Å·À­½Ç
  u8 pitch,roll,yaw; //Å·À­½Ç
u8 aacx,aacy,aacz; //¼ÓËٶȴ«¸ÐÆ÷ԭʼÊý¾Ý
u8 gyrox,gyroy,gyroz; //ÍÓÂÝÒÇԭʼÊý¾Ý
short acx,acy,acz; //¼ÓËٶȴ«¸ÐÆ÷ԭʼÊý¾Ý
short grox,groy,groz; //ÍÓÂÝÒÇԭʼÊý¾Ý

float rollkp=800,rollki=500,rollkd=30,pitchkp=500,pitchki=200,pitchkd=35,yawkp=700,yawki=30,yawkd=40;
dt_flag_t f; //ÐèÒª·¢ËÍÊý¾ÝµÄ±êÖ¾
u8 data_to_send[50]; //·¢ËÍÊý¾Ý»º´æ

/////////////////////////////////////////////////////////////////////////////////////
void ANO_DT_Send_Data(u8*data_to_send, u8 _cnt)
{ int i;
  for(i=0;i<_cnt;i ) usart1_send_char(data_to_send[i]);


}
//Data_Exchangeº¯Êý´¦Àí¸÷ÖÖÊý¾Ý·¢ËÍÇëÇ󣬱ÈÈçÏëʵÏÖÿ5ms·¢ËÍÒ»´Î´«¸ÐÆ÷Êý¾ÝÖÁÉÏλ»ú£¬¼´Ôڴ˺¯ÊýÄÚʵÏÖ
//´Ëº¯ÊýÓ¦ÓÉÓû§Ã¿1msµ÷ÓÃÒ»´Î
void ANO_DT_Data_Exchange(void)
{
static u8 cnt = 0;
static u8 senser_cnt = 10;
static u8 status_cnt = 15;
static u8 rcdata_cnt = 20;
static u8 motopwm_cnt = 20;
static u8 power_cnt = 50;
MPU_Get_Accelerometer(&acx,&acy,&acz); //µÃµ½¼ÓËٶȴ«¸ÐÆ÷Êý¾Ý
MPU_Get_Gyroscope(&grox,&groy,&groz); //µÃµ½ÍÓÂÝÒÇÊý¾Ý
mpu_dmp_get_data(&pit,&rol,&ya);
if((cnt % senser_cnt) == (senser_cnt-1))
f.send_senser = 1;

if((cnt % status_cnt) == (status_cnt-1))
f.send_status = 1;

if((cnt % rcdata_cnt) == (rcdata_cnt-1))
f.send_rcdata = 1;

if((cnt % motopwm_cnt) == (motopwm_cnt-1))
f.send_motopwm = 1;

if((cnt % power_cnt) == (power_cnt-1))
f.send_power = 1;

cnt ;
/////////////////////////////////////////////////////////////////////////////////////
if(f.send_version)  //°æ±¾ÐÅÏ¢ 
{
f.send_version = 0;
ANO_DT_Send_Version(4,300,100,400,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_status)  //pit roll yaw ¸ß¶È  ·ÉÐÐģʽ  0½âËø 1¼ÓËø
{
f.send_status = 0;
    roll=rol;
pitch=pit;
yaw=ya;
ANO_DT_Send_Status(rol,pit,ya,500,1,1);//¸ßµÄcm
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_senser)
{
f.send_senser = 0;
aacx=acx;
aacy=acy;
aacz=acz;
gyrox=grox;
gyroy=groy;
gyroz=groz;
ANO_DT_Send_Senser(aacx,aacy,aacz,
gyrox,gyroy,gyroz,
0,0,0,0);
}
/////////////////////////////////////////////////////////////////////////////////////
// else if(f.send_rcdata)
// {
// f.send_rcdata = 0;
// ANO_DT_Send_RCData(Rc_Pwm_In[0],Rc_Pwm_In[1],Rc_Pwm_In[2],Rc_Pwm_In[3],Rc_Pwm_In[4],Rc_Pwm_In[5],Rc_Pwm_In[6],Rc_Pwm_In[7],0,0);
// }
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_motopwm)
{
f.send_motopwm = 0;
ANO_DT_Send_MotoPWM(900,2,5,10000,8,6,6,8);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_power)
{
f.send_power = 0;
ANO_DT_Send_Power(234,456); //µçѹ µçÁ÷
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid1)
{
f.send_pid1 = 0;
ANO_DT_Send_PID(1,rollkp,rollki,rollkd,pitchkp,pitchki,pitchkd,yawkp,yawki,yawkd);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid2)
{
f.send_pid2 = 0;
//ANO_DT_Send_PID(2,p1_p,p1_i,p1_d,p2_p,p2_i,p2_d,p3_p,p3_i,p3_d);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid3)
{
f.send_pid3 = 0;
//ANO_DT_Send_PID(3,p1_p,p1_i,p1_d,p2_p,p2_i,p2_d,p3_p,p3_i,p3_d);
}
///////////////////////////////////////////////////////////////////////////////////////

}

///////////////////////////////////////////////////////////////////////////////////////
////Send_Dataº¯ÊýÊÇЭÒéÖÐËùÓз¢ËÍÊý¾Ý¹¦ÄÜʹÓõ½µÄ·¢Ëͺ¯Êý
////ÒÆֲʱ£¬Óû§Ó¦¸ù¾Ý×ÔÉíÓ¦ÓõÄÇé¿ö£¬¸ù¾ÝʹÓõÄͨÐÅ·½Ê½£¬ÊµÏִ˺¯Êý
//void ANO_DT_Send_Data(u8 *dataToSend , u8 length)
//{
//#ifdef ANO_DT_USE_USB_HID
// Usb_Hid_Adddata(data_to_send,length);
//#endif
//#ifdef ANO_DT_USE_USART2
// Usart2_Send(data_to_send, length);
//#endif
//}

static void ANO_DT_Send_Check(u8 head, u8 check_sum)
{  u8 sum = 0,i;
data_to_send[0]=0xAA;
data_to_send[1]=0xAA;
data_to_send[2]=0xEF;
data_to_send[3]=2;
data_to_send[4]=head;
data_to_send[5]=check_sum;



for(i=0;i<6;i )
sum = data_to_send[i];
data_to_send[6]=sum;

ANO_DT_Send_Data(data_to_send, 7);
}

/////////////////////////////////////////////////////////////////////////////////////
//Data_Receive_Prepareº¯ÊýÊÇЭÒéÔ¤½âÎö£¬¸ù¾ÝЭÒéµÄ¸ñʽ£¬½«ÊÕµ½µÄÊý¾Ý½øÐÐÒ»´Î¸ñʽÐÔ½âÎö£¬¸ñʽÕýÈ·µÄ»°ÔÙ½øÐÐÊý¾Ý½âÎö
//ÒÆֲʱ£¬´Ëº¯ÊýÓ¦ÓÉÓû§¸ù¾Ý×ÔÉíʹÓõÄͨÐÅ·½Ê½×ÔÐе÷Ó㬱ÈÈç´®¿ÚÿÊÕµ½Ò»×Ö½ÚÊý¾Ý£¬Ôòµ÷Óô˺¯ÊýÒ»´Î
//´Ëº¯Êý½âÎö³ö·ûºÏ¸ñʽµÄÊý¾ÝÖ¡ºó£¬»á×ÔÐе÷ÓÃÊý¾Ý½âÎöº¯Êý
void ANO_DT_Data_Receive_Prepare(u8 data)
{
static u8 RxBuffer[50];
static u8 _data_len = 0,_data_cnt = 0;
static u8 state = 0;

if(state==0&&data==0xAA)
{
state=1;
RxBuffer[0]=data;
}
else if(state==1&&data==0xAF)
{
state=2;
RxBuffer[1]=data;
}
else if(state==2&&data<0XF1)
{
state=3;
RxBuffer[2]=data;
}
else if(state==3&&data<50)
{
state = 4;
RxBuffer[3]=data;
_data_len = data;
_data_cnt = 0;
}
else if(state==4&&_data_len>0)
{
_data_len--;
RxBuffer[4 _data_cnt ]=data;
if(_data_len==0)
state = 5;
}
else if(state==5)
{
state = 0;
RxBuffer[4 _data_cnt]=data;
ANO_DT_Data_Receive_Anl(RxBuffer,_data_cnt 5);
}
else
state = 0;
}
/////////////////////////////////////////////////////////////////////////////////////
//Data_Receive_Anlº¯ÊýÊÇЭÒéÊý¾Ý½âÎöº¯Êý£¬º¯Êý²ÎÊýÊÇ·ûºÏЭÒé¸ñʽµÄÒ»¸öÊý¾ÝÖ¡£¬¸Ãº¯Êý»áÊ×ÏȶÔЭÒéÊý¾Ý½øÐÐУÑé
//УÑéͨ¹ýºó¶ÔÊý¾Ý½øÐнâÎö£¬ÊµÏÖÏàÓ¦¹¦ÄÜ
//´Ëº¯Êý¿ÉÒÔ²»ÓÃÓû§×ÔÐе÷Óã¬Óɺ¯ÊýData_Receive_Prepare×Ô¶¯µ÷ÓÃ
void ANO_DT_Data_Receive_Anl(u8 *data_buf,u8 num)
{
u8 sum = 0,i;
for(i=0;i<(num-1);i )
sum = *(data_buf i);
if(!(sum==*(data_buf num-1))) return; //ÅжÏsum
if(!(*(data_buf)==0xAA && *(data_buf 1)==0xAF)) return; //ÅжÏÖ¡Í·

// if(*(data_buf 2)==0X01)               //mpu6050µ÷Áã
// {
// if(*(data_buf 4)==0X01)
// mpu6050.Acc_CALIBRATE = 1;
// if(*(data_buf 4)==0X02)
// mpu6050.Gyro_CALIBRATE = 1;
// if(*(data_buf 4)==0X03)
// {
// mpu6050.Acc_CALIBRATE = 1;
// mpu6050.Gyro_CALIBRATE = 1;
// }
// }
//
if(*(data_buf 2)==0X02)     //¶ÁÈ¡pid
{
if(*(data_buf 4)==0X01)
{
f.send_pid1 = 1;
f.send_pid2 = 1;
f.send_pid3 = 1;
f.send_pid4 = 1;
f.send_pid5 = 1;
f.send_pid6 = 1;
}
if(*(data_buf 4)==0X02)
{

}
if(*(data_buf 4)==0XA0) //¶ÁÈ¡°æ±¾ÐÅÏ¢
{
f.send_version = 1;
}
if(*(data_buf 4)==0XA1) //»Ö¸´Ä¬ÈϲÎÊý
{
//Para_ResetToFactorySetup();
}
}

if(*(data_buf 2)==0X10) //PID1
    {
        rollkp  = 0.001*( (vs16)(*(data_buf 4)<<8)|*(data_buf 5) );
        rollki  = 0.001*( (vs16)(*(data_buf 6)<<8)|*(data_buf 7) );
        rollkd  = 0.001*( (vs16)(*(data_buf 8)<<8)|*(data_buf 9) );
        pitchkp = 0.001*( (vs16)(*(data_buf 10)<<8)|*(data_buf 11) );
        pitchki = 0.001*( (vs16)(*(data_buf 12)<<8)|*(data_buf 13) );
        pitchkd = 0.001*( (vs16)(*(data_buf 14)<<8)|*(data_buf 15) );
        yawkp = 0.001*( (vs16)(*(data_buf 16)<<8)|*(data_buf 17) );
        yawki = 0.001*( (vs16)(*(data_buf 18)<<8)|*(data_buf 19) );
        yawkd = 0.001*( (vs16)(*(data_buf 20)<<8)|*(data_buf 21) );
        ANO_DT_Send_Check(*(data_buf 2),sum);
//Param_SavePID();
    }
    if(*(data_buf 2)==0X11) //PID2
    {
//        ctrl_1.PID[PID4].kp = 0.001*( (vs16)(*(data_buf 4)<<8)|*(data_buf 5) );
//        ctrl_1.PID[PID4].ki = 0.001*( (vs16)(*(data_buf 6)<<8)|*(data_buf 7) );
//        ctrl_1.PID[PID4].kd = 0.001*( (vs16)(*(data_buf 8)<<8)|*(data_buf 9) );
//        ctrl_1.PID[PID5].kp = 0.001*( (vs16)(*(data_buf 10)<<8)|*(data_buf 11) );
//        ctrl_1.PID[PID5].ki = 0.001*( (vs16)(*(data_buf 12)<<8)|*(data_buf 13) );
//        ctrl_1.PID[PID5].kd = 0.001*( (vs16)(*(data_buf 14)<<8)|*(data_buf 15) );
//        ctrl_1.PID[PID6].kp  = 0.001*( (vs16)(*(data_buf 16)<<8)|*(data_buf 17) );
//        ctrl_1.PID[PID6].ki = 0.001*( (vs16)(*(data_buf 18)<<8)|*(data_buf 19) );
//        ctrl_1.PID[PID6].kd = 0.001*( (vs16)(*(data_buf 20)<<8)|*(data_buf 21) );
        ANO_DT_Send_Check(*(data_buf 2),sum);
//Param_SavePID();
    }
    if(*(data_buf 2)==0X12) //PID3
    {
//        ctrl_2.PID[PIDROLL].kp  = 0.001*( (vs16)(*(data_buf 4)<<8)|*(data_buf 5) );
//        ctrl_2.PID[PIDROLL].ki  = 0.001*( (vs16)(*(data_buf 6)<<8)|*(data_buf 7) );
//        ctrl_2.PID[PIDROLL].kd  = 0.001*( (vs16)(*(data_buf 8)<<8)|*(data_buf 9) );
//        ctrl_2.PID[PIDPITCH].kp = 0.001*( (vs16)(*(data_buf 10)<<8)|*(data_buf 11) );
//        ctrl_2.PID[PIDPITCH].ki = 0.001*( (vs16)(*(data_buf 12)<<8)|*(data_buf 13) );
//        ctrl_2.PID[PIDPITCH].kd = 0.001*( (vs16)(*(data_buf 14)<<8)|*(data_buf 15) );
//        ctrl_2.PID[PIDYAW].kp = 0.001*( (vs16)(*(data_buf 16)<<8)|*(data_buf 17) );
//        ctrl_2.PID[PIDYAW].ki = 0.001*( (vs16)(*(data_buf 18)<<8)|*(data_buf 19) );
//        ctrl_2.PID[PIDYAW].kd = 0.001*( (vs16)(*(data_buf 20)<<8)|*(data_buf 21) );
        ANO_DT_Send_Check(*(data_buf 2),sum);
//Param_SavePID();
    }
if(*(data_buf 2)==0X13) //PID4
{
ANO_DT_Send_Check(*(data_buf 2),sum);
}
if(*(data_buf 2)==0X14) //PID5
{
ANO_DT_Send_Check(*(data_buf 2),sum);
}
if(*(data_buf 2)==0X15) //PID6
{
ANO_DT_Send_Check(*(data_buf 2),sum);
}


}

void ANO_DT_Send_Version(u8 hardware_type, u16 hardware_ver,u16 software_ver,u16 protocol_ver,u16 bootloader_ver)
{
u8 _cnt=0,i,sum = 0;
data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0x00;
data_to_send[_cnt ]=0;

data_to_send[_cnt ]=hardware_type;
data_to_send[_cnt ]=BYTE1(hardware_ver);
data_to_send[_cnt ]=BYTE0(hardware_ver);
data_to_send[_cnt ]=BYTE1(software_ver);
data_to_send[_cnt ]=BYTE0(software_ver);
data_to_send[_cnt ]=BYTE1(protocol_ver);
data_to_send[_cnt ]=BYTE0(protocol_ver);
data_to_send[_cnt ]=BYTE1(bootloader_ver);
data_to_send[_cnt ]=BYTE0(bootloader_ver);

data_to_send[3] = _cnt-4;


for(i=0;i<_cnt;i )
sum = data_to_send[i];
data_to_send[_cnt ]=sum;

ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Status(float angle_rol, float angle_pit, float angle_yaw, s32 alt, u8 fly_model, u8 armed)
{
u8 _cnt=0;
vs16 _temp;
vs32 _temp2 = alt;
int i;
int sum = 0;

data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0x01;
data_to_send[_cnt ]=0;

_temp = (int)(angle_rol*100);
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = (int)(angle_pit*100);
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = (int)(angle_yaw*100);
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);

data_to_send[_cnt ]=BYTE3(_temp2);
data_to_send[_cnt ]=BYTE2(_temp2);
data_to_send[_cnt ]=BYTE1(_temp2);
data_to_send[_cnt ]=BYTE0(_temp2);

data_to_send[_cnt ] = fly_model;

data_to_send[_cnt ] = armed;

data_to_send[3] = _cnt-4;

//  int sum = 0;
for(i=0;i<_cnt;i )
sum = data_to_send[i];
data_to_send[_cnt ]=sum;

//for(i=0;i<_cnt;i ) usart1_send_char(data_to_send[i]);
ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Senser(s16 a_x,s16 a_y,s16 a_z,s16 g_x,s16 g_y,s16 g_z,s16 m_x,s16 m_y,s16 m_z,s32 bar)
{
u8 _cnt=0,sum = 0,i;
vs16 _temp;

data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0x02;
data_to_send[_cnt ]=0;

_temp = a_x;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = a_y;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = a_z;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);

_temp = g_x;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = g_y;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = g_z;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);

_temp = m_x;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = m_y;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = m_z;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);

data_to_send[3] = _cnt-4;

 
for(i=0;i<_cnt;i )
sum = data_to_send[i];
data_to_send[_cnt ] = sum;

ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_RCData(u16 thr,u16 yaw,u16 rol,u16 pit,u16 aux1,u16 aux2,u16 aux3,u16 aux4,u16 aux5,u16 aux6)
{
u8 _cnt=0,i,sum = 0;

data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0x03;
data_to_send[_cnt ]=0;
data_to_send[_cnt ]=BYTE1(thr);
data_to_send[_cnt ]=BYTE0(thr);
data_to_send[_cnt ]=BYTE1(yaw);
data_to_send[_cnt ]=BYTE0(yaw);
data_to_send[_cnt ]=BYTE1(rol);
data_to_send[_cnt ]=BYTE0(rol);
data_to_send[_cnt ]=BYTE1(pit);
data_to_send[_cnt ]=BYTE0(pit);
data_to_send[_cnt ]=BYTE1(aux1);
data_to_send[_cnt ]=BYTE0(aux1);
data_to_send[_cnt ]=BYTE1(aux2);
data_to_send[_cnt ]=BYTE0(aux2);
data_to_send[_cnt ]=BYTE1(aux3);
data_to_send[_cnt ]=BYTE0(aux3);
data_to_send[_cnt ]=BYTE1(aux4);
data_to_send[_cnt ]=BYTE0(aux4);
data_to_send[_cnt ]=BYTE1(aux5);
data_to_send[_cnt ]=BYTE0(aux5);
data_to_send[_cnt ]=BYTE1(aux6);
data_to_send[_cnt ]=BYTE0(aux6);

data_to_send[3] = _cnt-4;


for(i=0;i<_cnt;i )
sum = data_to_send[i];

data_to_send[_cnt ]=sum;

ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Power(u16 votage, u16 current)
{
u8 _cnt=0,i,sum=0;
u16 temp;

data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0x05;
data_to_send[_cnt ]=0;

temp = votage;
data_to_send[_cnt ]=BYTE1(temp);
data_to_send[_cnt ]=BYTE0(temp);
temp = current;
data_to_send[_cnt ]=BYTE1(temp);
data_to_send[_cnt ]=BYTE0(temp);

data_to_send[3] = _cnt-4;


for(i=0;i<_cnt;i )
sum = data_to_send[i];

data_to_send[_cnt ]=sum;

ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_MotoPWM(u16 m_1,u16 m_2,u16 m_3,u16 m_4,u16 m_5,u16 m_6,u16 m_7,u16 m_8)
{
u8 _cnt=0,i,sum = 0;

data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0x06;
data_to_send[_cnt ]=0;

data_to_send[_cnt ]=BYTE1(m_1);
data_to_send[_cnt ]=BYTE0(m_1);
data_to_send[_cnt ]=BYTE1(m_2);
data_to_send[_cnt ]=BYTE0(m_2);
data_to_send[_cnt ]=BYTE1(m_3);
data_to_send[_cnt ]=BYTE0(m_3);
data_to_send[_cnt ]=BYTE1(m_4);
data_to_send[_cnt ]=BYTE0(m_4);
data_to_send[_cnt ]=BYTE1(m_5);
data_to_send[_cnt ]=BYTE0(m_5);
data_to_send[_cnt ]=BYTE1(m_6);
data_to_send[_cnt ]=BYTE0(m_6);
data_to_send[_cnt ]=BYTE1(m_7);
data_to_send[_cnt ]=BYTE0(m_7);
data_to_send[_cnt ]=BYTE1(m_8);
data_to_send[_cnt ]=BYTE0(m_8);

data_to_send[3] = _cnt-4;


for(i=0;i<_cnt;i )
sum = data_to_send[i];

data_to_send[_cnt ]=sum;

ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_PID(u8 group,float p1_p,float p1_i,float p1_d,float p2_p,float p2_i,float p2_d,float p3_p,float p3_i,float p3_d)
{
u8 _cnt=0,i,sum = 0;
vs16 _temp;

data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0xAA;
data_to_send[_cnt ]=0x10 group-1;
data_to_send[_cnt ]=0;


_temp = p1_p ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = p1_i  ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = p1_d  ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = p2_p  ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = p2_i  ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = p2_d ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = p3_p ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = p3_i ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);
_temp = p3_d ;
data_to_send[_cnt ]=BYTE1(_temp);
data_to_send[_cnt ]=BYTE0(_temp);

data_to_send[3] = _cnt-4;


for(i=0;i<_cnt;i )
sum = data_to_send[i];

data_to_send[_cnt ]=sum;

ANO_DT_Send_Data(data_to_send, _cnt);
}

/******************* (C) COPYRIGHT 2014 ANO TECH *****END OF FILE************/


标签： STM32 2

实例下载地址

stm32四轴飞行器mpu6050+飞控

不能下载？内容有错？ 点击这里报错 + 投诉 + 提问

好例子网口号：伸出你的我的手 — 分享！

相关软件

相关文章

网友评论

我要评论

发表评论

(您的评论需要经过审核才能显示)

查看所有0条评论>>

小贴士

感谢您为本站写下的评论，您的评论对其它用户来说具有重要的参考价值，所以请认真填写。

  • 类似“顶”、“沙发”之类没有营养的文字，对勤劳贡献的楼主来说是令人沮丧的反馈信息。
  • 相信您也不想看到一排文字/表情墙，所以请不要反馈意义不大的重复字符，也请尽量不要纯表情的回复。
  • 提问之前请再仔细看一遍楼主的说明，或许是您遗漏了。
  • 请勿到处挖坑绊人、招贴广告。既占空间让人厌烦，又没人会搭理，于人于己都无利。

关于好例子网

本站旨在为广大IT学习爱好者提供一个非营利性互相学习交流分享平台。本站所有资源都可以被免费获取学习研究。本站资源来自网友分享，对搜索内容的合法性不具有预见性、识别性、控制性，仅供学习研究，请务必在下载后24小时内给予删除，不得用于其他任何用途，否则后果自负。基于互联网的特殊性，平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查；无论平台是否已进行审查，用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场，基于网友分享，根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定，若资源存在侵权或相关问题请联系本站客服人员，点此联系我们。关于更多版权及免责申明参见 版权及免责申明

;
报警