四轴飞行器、无人机(STM32、NRF24L01)

一、简介

    此电路由STM32为主控芯片，NRF24L01、MPU6050为辅,当接受到信号时，处理对应的指令。

二、实物图

三、部分代码

#include "ALL_DATA.h"
#include "mpu6050.h"
#include "I2C.h"
#include "filter.h"
#include <string.h>
#include "LED.h"
#include "myMath.h"
#include "kalman.h"
#include "flash.h"


#define	SMPLRT_DIV		0x19	//ÍÓÂÝÒDzÉÑùÂÊ£¬µäÐÍÖµ£º0x07(125Hz)
#define	CONFIGL			0x1A	//µÍͨÂ˲¨ÆµÂÊ£¬µäÐÍÖµ£º0x06(5Hz)
#define	GYRO_CONFIG		0x1B	//ÍÓÂÝÒÇ×Լ켰²âÁ¿·¶Î§£¬µäÐÍÖµ£º0x18(²»×Լ죬2000deg/s)
#define	ACCEL_CONFIG	0x1C	//¼ÓËÙ¼Æ×Լ졢²âÁ¿·¶Î§¼°¸ßͨÂ˲¨ÆµÂÊ£¬µäÐÍÖµ£º0x01(²»×Լ죬2G£¬5Hz)
#define	ACCEL_ADDRESS	0x3B
#define	ACCEL_XOUT_H	0x3B
#define	ACCEL_XOUT_L	0x3C
#define	ACCEL_YOUT_H	0x3D
#define	ACCEL_YOUT_L	0x3E
#define	ACCEL_ZOUT_H	0x3F
#define	ACCEL_ZOUT_L	0x40
#define	TEMP_OUT_H		0x41
#define	TEMP_OUT_L		0x42
#define	GYRO_XOUT_H		0x43
#define GYRO_ADDRESS  0x43
#define	GYRO_XOUT_L		0x44	
#define	GYRO_YOUT_H		0x45
#define	GYRO_YOUT_L		0x46
#define	GYRO_ZOUT_H		0x47
#define	GYRO_ZOUT_L		0x48
#define	PWR_MGMT_1		0x6B	//µçÔ´¹ÜÀí£¬µäÐÍÖµ£º0x00(Õý³£ÆôÓÃ)
#define	WHO_AM_I		  0x75	//IICµØÖ·¼Ä´æÆ÷(ĬÈÏÊýÖµ0x68£¬Ö»¶Á)
#define MPU6050_PRODUCT_ID 0x68
#define MPU6050_ADDRESS 0xD0//0x68



int16_t MpuOffset[6] = {0};

static volatile int16_t *pMpu = (int16_t *)&MPU6050;



/****************************************************************************************
MPU6050¸´Î»
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
int8_t mpu6050_rest(void)
{
	if(IIC_Write_One_Byte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80) == FAILED)
		return FAILED;	//¸´Î»
	delay_ms(20);
	return SUCCESS;
}
/****************************************************************************************
MPU6050³õʼ»¯
*@brief   
*@brief  
*@param[in]
*****************************************************************************************/
int8_t MpuInit(void) //³õʼ»¯
{
	uint8_t date = SUCCESS;
	do
	{
	date = IIC_Write_One_Byte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80);	//¸´Î»
	delay_ms(30);
	date += IIC_Write_One_Byte(MPU6050_ADDRESS, SMPLRT_DIV, 0x02); //ÍÓÂÝÒDzÉÑùÂÊ£¬0x00(500Hz)
	date += IIC_Write_One_Byte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);	//ÉèÖÃÉ豸ʱÖÓÔ´£¬ÍÓÂÝÒÇZÖá
	date += IIC_Write_One_Byte(MPU6050_ADDRESS, CONFIGL, 0x03);   //µÍͨÂ˲¨ÆµÂÊ£¬0x03(42Hz)
	date += IIC_Write_One_Byte(MPU6050_ADDRESS, GYRO_CONFIG, 0x18);//+-2000deg/s
	date += IIC_Write_One_Byte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x09);//+-4G
	}
	while(date != SUCCESS);  //Èç¹ûÍÓÂÝÒÇÕý³£
	date = IIC_Read_One_Byte(MPU6050_ADDRESS, 0x75);  //ÅжÏMPU6050µØÖ·
	if(date!= MPU6050_PRODUCT_ID) //Èç¹ûµØÖ·ÕýÈ·
		return FAILED;
	FLASH_read(MpuOffset,6);//´ÓmcuµÄFLASHÖжÁÈ¡MPU6050µÄˮƽ¾²Ö¹±ê¶¨Ð£×¼Öµ
	return SUCCESS;
}
/****************************************************************************************
»ñÈ¡MPU6050Êý¾Ý
*@brief    
*@brief   
*@param[in]
*****************************************************************************************/

#define  Acc_Read() IIC_read_Bytes(MPU6050_ADDRESS, 0X3B,buffer,6) //¶ÁÈ¡¼ÓËÙ¶È
#define  Gyro_Read() IIC_read_Bytes(MPU6050_ADDRESS, 0x43,&buffer[6],6)  //  ¶ÁÈ¡½ÇËÙ¶È

void MpuGetData(void) //¶ÁÈ¡ÍÓÂÝÒÇÊý¾Ý¼ÓÂ˲¨
{
	  uint8_t i;
    uint8_t buffer[12];

		Acc_Read();
	  Gyro_Read();

		for(i=0;i<6;i++)
		{
			pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];		
			if(i < 3) //´Ë´¦¶Ô¼ÓËÙ¶È×öһά¿¨¶ûÂüÂ˲¨
			{
				{
					static struct _1_ekf_filter ekf[3] = {{0.02,0,8192,0,0.001,0.543},{0.02,0,8192,0,0.001,0.543},{0.02,0,8192,0,0.001,0.543}};	
					kalman_1(&ekf[i],(float)pMpu[i]);  //һά¿¨¶ûÂü
					pMpu[i] = (int16_t)ekf[i].out;
				}
		}
		if(i > 2)  //´Ë´¦¶Ô½ÇËÙ¶È×öÒ»½éµÍͨÂ˲¨
		{	
			uint8_t k=i-3;
			const float factor = 0.25f;  //Â˲¨ÒòËØ			
			static float tBuff[3];		

			pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;                
		}
	}
}

/****************************************************************************************
*@brief   get mpu offset
*@brief   initial and cmd call this
*@param[in]
*****************************************************************************************/
void MpuGetOffset(void) //У׼
{
	int32_t buffer[6]={0};
	int16_t i;  
	uint8_t k=30;
	const int8_t MAX_GYRO_QUIET = 5;
	const int8_t MIN_GYRO_QUIET = -5;	
/*           wait for calm down    	                                                          */
	int16_t LastGyro[3] = {0};
	int16_t ErrorGyro[3];	
	/*           set offset initial to zero    		*/
	
	memset(MpuOffset,0,12);
	MpuOffset[2] = 8192;   //¸ù¾ÝÊÖ²áÁ¿³ÌÉ趨¼ÓËٶȱ궨ֵ 
	
	TIM_ITConfig(  //ʹÄÜ»òÕßʧÄÜÖ¸¶¨µÄTIMÖжÏ
		TIM3, //TIM2
		TIM_IT_Update ,
		DISABLE  //ʹÄÜ
		);	
	while(k--)  //ÅжϷɿØÊÇ·ñ´¦ÓÚ¾²Ö¹×´Ì¬
	{
		do
		{
			delay_ms(10);
			MpuGetData();
			for(i=0;i<3;i++)
			{
				ErrorGyro[i] = pMpu[i+3] - LastGyro[i];
				LastGyro[i] = pMpu[i+3];	
			}			
		}while ((ErrorGyro[0] >  MAX_GYRO_QUIET )|| (ErrorGyro[0] < MIN_GYRO_QUIET)
					||(ErrorGyro[1] > MAX_GYRO_QUIET )|| (ErrorGyro[1] < MIN_GYRO_QUIET)
					||(ErrorGyro[2] > MAX_GYRO_QUIET )|| (ErrorGyro[2] < MIN_GYRO_QUIET)
						);
	}	

/*           throw first 100  group data and make 256 group average as offset                    */	
	for(i=0;i<356;i++)  //È¡µÚ100µ½µÚ356×éµÄƽ¾ùÖµ×öΪУ׼ֵ
	{		
		MpuGetData();
		if(100 <= i)
		{
			uint8_t k;
			for(k=0;k<6;k++)
			{
				buffer[k] += pMpu[k];
			}
		}
	}

	for(i=0;i<6;i++)  //±£´æУ׼ֵ
	{
		MpuOffset[i] = buffer[i]>>8;
	}
	TIM_ITConfig(  //ʹÄÜ»òÕßʧÄÜÖ¸¶¨µÄTIMÖжÏ
		TIM3, //TIM2
		TIM_IT_Update ,
		ENABLE  //ʹÄÜ
		);
	FLASH_write(MpuOffset,6);//½«Êý¾Ýдµ½FLASHÖУ¬Ò»¹²ÓÐ6¸öint16Êý¾Ý	
}