DIY STM32小四轴飞行器

飞机部分

硬件设计

电源设计

STM32主控

MPU6050

电机控制

蓝牙、2.4G

其他模块

BOM表

Comment	Description	Designator	Footprint	Quantity	Value
CAP	0603电容	C1, C2, C3, C4, C5, C9, C10, C11, C16, C17, C18, C19	C_0603	12	100nF
CAP	0603电容	C6, C7	C_0603	2	22pF
Cap	0603电容	C8	C_0603	1	2.2nF
47uF	1206钽电容	C12, C13, C20	C_1206	3	100pF
CAP	0603电容	C14, C15, C21	C_0603	3	100nF
S4	二极管	D1, D2	SOD-123	2
DC4.2V	2p排针	JP1	SIP2	1
MSS22D18	6p贴片拨动开关	KG1	MSS22D18	1
Inductor Chip	CD32电感	L1	47UH_3*3 150MA	1	33UH
蓝灯	发光二极管	LED1, LED3	0603_LED_S1	2
红灯	发光二极管	LED2, LED4	0603_LED_S1	2
Header 2	焊接电机用，不用器件	MOTO1, MOTO2, MOTO3, MOTO4	SIP2D - duplicate	4
NRF24L01	2.4G模块	NRF1	24L01	1
串口	4p排针	P1	SIP4 - duplicate	1
IIC	4p排针	P2	SIP4 - duplicate	1
SW下载	4p排针	P3	SIP4 - duplicate	1
Header 4	4p排针	P4	SIP4 - duplicate	1
SI2302	SI2302场效应管	Q1, Q2, Q3, Q4	SOT-23(SOT-23-3)	4
Res2	0603电阻	R1	R0603	1	10K
Res2	0603电阻	R2, R3, R7, R9, R11, R13	R0603	6	10K
Res2	0603电阻	R4, R5, R6, R8, R10, R12, R14, R15	R0603	8	1K
STM32F103C8T6_LQFP48	STM32F103C8T6_LQFP48	U1	LQFP48_STM32	1
MPU6050	MPU6050	U2		1
BL8530	电源芯片	U3	SOT-89(SOT-89-3)	1
662K	LDO	U4	SOT-23-3L	1
BAT54A	BAT54A肖特基二极管	U5, U6, U7, U8	SOT-23(SOT-23-3)	4
XTAL	贴片晶振CSTCE8M00G55	Y1	CSTCE8M00G55	1	8M

软件设计

main.c

#include "ALL_DEFINE.h"
#include "init.h"

int main(void)
{	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //2个bit的抢占优先级，2个bit的子优先级
	ALL_Init();//系统初始化
	while(1)
	{
		ANTO_polling(); 	//匿名上位机
		LEDRefresh(); 			//LED刷新
	}
}

delay.c

#include "stm32f10x.h"
#include "misc.h"
#include "delay.h"
#include "ALL_DATA.h"
static volatile uint32_t usTicks = 0;

void cycleCounterInit(void)
{
    RCC_ClocksTypeDef clocks;
    RCC_GetClocksFreq(&clocks);
    usTicks = clocks.SYSCLK_Frequency / 1000000;
}


uint32_t GetSysTime_us(void) 
{
    register uint32_t ms, cycle_cnt;
    do {
        ms = SysTick_count;
        cycle_cnt = SysTick->VAL;
    	} while (ms != SysTick_count);
    return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
}

//    毫秒级延时函数	 
void delay_ms(uint16_t nms)
{
	uint32_t t0=GetSysTime_us();
	while(GetSysTime_us() - t0 < nms * 1000);	  	  
}

void delay_us(unsigned int i)
 {  
	char x=0;   
    while( i--)
    {	
       for(x=1;x>0;x--);
    }
 }		  

Init.c

#include "ALL_DEFINE.h"

volatile uint32_t SysTick_count; //系统时间计数
_st_Mpu MPU6050;   //MPU6050原始数据
_st_Mag AK8975;
_st_AngE Angle;    //当前角度姿态值
_st_Remote Remote; //遥控通道值


_st_ALL_flag ALL_flag; //系统标志位，包含解锁标志位等


PidObject pidRateX; //内环PID数据
PidObject pidRateY;
PidObject pidRateZ;

PidObject pidPitch; //外环PID数据
PidObject pidRoll;
PidObject pidYaw;

PidObject pidHeightRate;
PidObject pidHeightHigh;

void pid_param_Init(void); //PID控制参数初始化，改写PID并不会保存数据，请调试完成后直接在程序里更改 再烧录到飞控


void ALL_Init(void)
{
	cycleCounterInit();  //得到系统每个us的系统CLK个数，为以后延时函数，和得到精准的当前执行时间使用
	SysTick_Config(SystemCoreClock / 1000);	//系统滴答时钟
	IIC_Init();             //I2C初始化
	pid_param_Init();       //PID参数初始化	 
	LEDInit();              //LED闪灯初始化
	MpuInit();              //MPU6050初始化	
	USART3_Config();        //上位机串口初始化
	NRF24L01_init();				//2.4G遥控通信初始化	
	TIM2_PWM_Config();			//4路PWM初始化	
	TIM3_Config();					//系统工作周期初始化 
}

void pid_param_Init(void)
{
	pidRateX.kp = 2.0f;
	pidRateY.kp = 2.0f;
	pidRateZ.kp = 4.0f;
	
	pidRateX.ki = 0.0f;
	pidRateY.ki = 0.0f;
	pidRateZ.ki = 0.0f;	
	
	pidRateX.kd = 0.08f;
	pidRateY.kd = 0.08f;
	pidRateZ.kd = 0.5f;	
	
	pidPitch.kp = 7.0f;
	pidRoll.kp = 7.0f;
	pidYaw.kp = 7.0f;	
}

IIC.c

#include "i2c.h"
#include "ALL_DEFINE.h"
#undef SUCCESS
#define SUCCESS 0
#undef FAILED
#define FAILED  1

/******************************************************************************
 * 函数名称: I2c_delay
 * 函数功能: I2c 延时函数
 * 入口参数: 无
 ******************************************************************************/
void I2c_delay(void) 
{
	volatile unsigned char i = 1;
	while (i)
	i--;\
}

/******************************************************************************
 * 函数名称: I2c_Init
 * 函数功能: I2c  GPIO初始化
 * 入口参数: 无
 ******************************************************************************/
 void IIC_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStrucSUCCESS;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB  , ENABLE);
		
	GPIO_InitStrucSUCCESS.GPIO_Pin = SCL_PIN | SDA_PIN;
	GPIO_InitStrucSUCCESS.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStrucSUCCESS.GPIO_Mode = GPIO_Mode_Out_OD;
	GPIO_Init(IIC_GPIO, &GPIO_InitStrucSUCCESS);
}

/******************************************************************************
 * 函数名称: I2c_Start
 * 函数功能: I2c  起始信号
 * 入口参数: 无
 ******************************************************************************/
static uint8_t I2c_Start(void)
{
	SDA_H;
	SCL_H;
	I2c_delay();
	if (!SDA_read)
		return FAILED;
	SDA_L;
	I2c_delay();
	if (SDA_read)
		return FAILED;
	SCL_L;
	I2c_delay();
	return SUCCESS;
}

/******************************************************************************
 * 函数名称: I2c_Stop
 * 函数功能: I2c  停止信号
 * 入口参数: 无
 ******************************************************************************/
static void I2c_Stop(void)
{
	SCL_L;
	I2c_delay();
	SDA_L;
	I2c_delay();
	I2c_delay();
	SCL_H;
	I2c_delay();
	SDA_H;
	I2c_delay();
}

/******************************************************************************
 * 函数名称: I2c_Ack
 * 函数功能: I2c  产生应答信号
 * 入口参数: 无
 ******************************************************************************/
static void I2c_Ack(void)
{
	SCL_L;
	I2c_delay();
	SDA_L;
	I2c_delay();
	SCL_H;
	I2c_delay();
	I2c_delay();
	I2c_delay();
	I2c_delay();
	SCL_L;
	I2c_delay();
}

/******************************************************************************
 * 函数名称: I2c_NoAck
 * 函数功能: I2c  产生NAck
 * 入口参数: 无
 ******************************************************************************/
static void I2c_NoAck(void)
{
	SCL_L;
	I2c_delay();
	SDA_H;
	I2c_delay();
	SCL_H;
	I2c_delay();
	I2c_delay();
	I2c_delay();
	I2c_delay();
	SCL_L;
	I2c_delay();
}

/*******************************************************************************
 *函数名称:	I2c_WaitAck
 *函数功能:	等待应答信号到来
 *返回值：   1，接收应答失败
 *           0，接收应答成功
 *******************************************************************************/
static uint8_t I2c_WaitAck(void)
{
	SCL_L;
	I2c_delay();
	SDA_H;
	I2c_delay();
	SCL_H;
	I2c_delay();
	I2c_delay();
	I2c_delay();
	if (SDA_read) 
	{
		SCL_L;
		return FAILED;
	}
	SCL_L;
	return SUCCESS;
}

/******************************************************************************
 * 函数名称: I2c_SendByte
 * 函数功能: I2c  发送一个字节数据
 * 入口参数: byte  发送的数据
 ******************************************************************************/
static void I2c_SendByte(uint8_t byte)
{
	uint8_t i = 8;
	while (i--) 
	{
		SCL_L;
		I2c_delay();
		if (byte & 0x80)
			SDA_H;
		else
			SDA_L;
		byte <<= 1;
		I2c_delay();
		SCL_H;
		I2c_delay();
		I2c_delay();
		I2c_delay();
	}
	SCL_L;
}

/******************************************************************************
 * 函数名称: I2c_ReadByte
 * 函数功能: I2c  读取一个字节数据
 * 入口参数: 无
 * 返回值	 读取的数据
 ******************************************************************************/
static uint8_t I2c_ReadByte(void)
{
	uint8_t i = 8;
	uint8_t byte = 0;

	SDA_H;
	while (i--) 
	{
		byte <<= 1;
		SCL_L;
		I2c_delay();
		I2c_delay();
		SCL_H;
		I2c_delay();
		I2c_delay();
		I2c_delay();
		if (SDA_read) 
		{
			byte |= 0x01;
		}
   }
	SCL_L;
	return byte;
}

/******************************************************************************
 * 函数名称: i2cWriteBuffer
 * 函数功能: I2c       向设备的某一个地址写入固定长度的数据
 * 入口参数: addr,     设备地址
 *           reg，     寄存器地址
 *			 len，     数据长度
 *			 *data	   数据指针
 * 返回值	 1
 ******************************************************************************/
int8_t IIC_Write_Bytes(uint8_t addr,uint8_t reg,uint8_t *data,uint8_t len)
{
	int i;
	if (I2c_Start() == FAILED)
		return FAILED;
	I2c_SendByte(addr);
	if (I2c_WaitAck() == FAILED) 
	{
			I2c_Stop();
			return FAILED;
	}
	I2c_SendByte(reg);
	I2c_WaitAck();
	for (i = 0; i < len; i++) 
	{
		I2c_SendByte(data[i]);
		if (I2c_WaitAck() == FAILED) 
		{
				I2c_Stop();
				return FAILED;
		}
	}
	I2c_Stop();
	return SUCCESS;
}
int8_t IIC_Read_One_Byte(uint8_t addr,uint8_t reg)
{
	uint8_t recive = 0;
	if (I2c_Start() == FAILED)
		return FAILED;
	I2c_SendByte(addr);
	if (I2c_WaitAck() == FAILED) 
	{
		I2c_Stop();
		return FAILED;
	}
	I2c_SendByte(reg);
	I2c_WaitAck();
	I2c_Stop();
	I2c_Start();
	I2c_SendByte(addr+1);
	if (I2c_WaitAck() == FAILED) 
	{
		I2c_Stop();
		return FAILED;
	}
	recive = I2c_ReadByte();
	I2c_NoAck();
	I2c_Stop();
	return recive;
}

/*****************************************************************************
 *函数名称:	i2cWrite
 *函数功能:	写入指定设备 指定寄存器一个字节
 *入口参数： addr 目标设备地址
 *		     reg   寄存器地址
 *		     data 读出的数据将要存放的地址
 *******************************************************************************/
int8_t IIC_Write_One_Byte(uint8_t addr,uint8_t reg,uint8_t data)
{
	if (I2c_Start() == FAILED)
		return FAILED;
	I2c_SendByte(addr);
	if (I2c_WaitAck() == FAILED) 
	{
		I2c_Stop();
		return FAILED;
	}
	I2c_SendByte(reg);
	I2c_WaitAck();
	I2c_SendByte(data);
	I2c_WaitAck();
	I2c_Stop();
	return SUCCESS;
}

int8_t IIC_read_Bytes(uint8_t addr,uint8_t reg,uint8_t *data,uint8_t len)
{
	if (I2c_Start() == FAILED)
		return FAILED;
	I2c_SendByte(addr);
	if (I2c_WaitAck() == FAILED) 
	{
		I2c_Stop();
		return FAILED;
	}
	I2c_SendByte(reg);
	I2c_WaitAck();
	I2c_Stop();
	I2c_Start();
	I2c_SendByte(addr+1);
	if (I2c_WaitAck() == FAILED) 
	{
		I2c_Stop();
		return FAILED;
	}
	while (len) 
	{
		*data = I2c_ReadByte();
		if (len == 1)
				I2c_NoAck();
		else
				I2c_Ack();
		data++;
		len--;
	}
	I2c_Stop();
	return SUCCESS;
}

Usart.c

#include "usart.h"
#include "misc.h"
#include "stdio.h"

/*
 * 函数名：USART1_Config
 * 描述  ：USART1 GPIO 配置,工作模式配置。
 * 输入  ：无
 * 输出  : 无
 * 调用  ：外部调用
 */
void USART3_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure; 
	/* 配置串口1 （USART1） 时钟*/
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3 | RCC_APB2Periph_GPIOB, ENABLE);

	/* 使能串口1中断 */
	NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;	//USART1  串口1全局中断 
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;//抢占优先级1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; //子优先级1
	/*IRQ通道使能*/
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	/*根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器USART1*/
	NVIC_Init(&NVIC_InitStructure);
	
	/*串口GPIO端口配置*/
  /* 配置串口1 （USART1 Tx (PA.09)）*/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
  
	/* 配置串口1 USART1 Rx (PA.10)*/
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
		
	//USART 初始化设置
	USART_InitStructure.USART_BaudRate =500000;//串口波特率
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
	USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//收发模式
	USART_Init(USART3, &USART_InitStructure);
	
	USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//开启中断
	
	USART_Cmd(USART3, ENABLE); //使能串口 
}


void USART1_SendByte(const int8_t *Data,uint8_t len)
{ 
	uint8_t i;
	
	for(i=0;i<len;i++)
	{
		while (!(USART1->SR & USART_FLAG_TXE));	 // while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
		USART_SendData(USART1,*(Data+i));	 
	}
}

void USART3_SendByte(const int8_t *Data,uint8_t len)
{ 
	uint8_t i;
	
	for(i=0;i<len;i++)
	{
		while (!(USART3->SR & USART_FLAG_TXE));	 // while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
		USART_SendData(USART3,*(Data+i));	 
	}
}


int8_t CheckSend[7]={0xAA,0XAA,0xEF,2,0,0,0};

void USART1_setBaudRate(uint32_t baudRate)
{
	USART_InitTypeDef USART_InitStructure;
	USART_InitStructure.USART_BaudRate =  baudRate;
	USART_Init(USART1, &USART_InitStructure);
}

/*
 * 函数名：fputc
 * 描述  ：重定向c库函数printf到USART1
 * 输入  ：无
 * 输出  ：无
 * 调用  ：由printf调用
 */
int fputc(int ch, FILE *f)
{
	/* 将Printf内容发往串口 */
	USART_SendData(USART1, (unsigned char) ch);
	while( USART_GetFlagStatus(USART1,USART_FLAG_TC)!= SET);	
	return (ch);
}

TIM.c

#include "stm32f10x.h"
#include "delay.h"
#include "TIM.h"
#include "ALL_DEFINE.h"

void TIM2_PWM_Config(void)
{
  TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
  TIM_OCInitTypeDef TIM_OCInitStructure;
  GPIO_InitTypeDef GPIO_InitStructure;
  /* 使能GPIOA时钟时钟 */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 |
    GPIO_Pin_3;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_Init(GPIOA, &GPIO_InitStructure);
  /* 使能定时器2时钟 */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
  /* Time base configuration */
  TIM_TimeBaseStructure.TIM_Period = 999; //定时器计数周期 0-999  1000	
  TIM_TimeBaseStructure.TIM_Prescaler = 8; //设置预分频：8+1分频   8K PWM频率
  TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分频系数：不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上计数模式

  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

  /* PWM1 Mode configuration: Channel */
  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //配置为PWM模式1
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_Pulse = 0;
    //设置跳变值，当计数器计数到这个值时，电平发生跳变(即占空比) 初始值0
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    //当定时器计数值小于定时设定值时为高电平
  /* 使能通道1 */
  TIM_OC1Init(TIM2, &TIM_OCInitStructure);
  TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
  /* 使能通道2 */
  TIM_OC2Init(TIM2, &TIM_OCInitStructure);
  TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
  /* 使能通道3 */
  TIM_OC3Init(TIM2, &TIM_OCInitStructure);
  TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
  /* 使能通道4 */
  TIM_OC4Init(TIM2, &TIM_OCInitStructure);
  TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);

  TIM_ARRPreloadConfig(TIM2, ENABLE); // 使能TIM2重载寄存器ARR
  TIM_Cmd(TIM2, ENABLE); //使能定时器2
}



//通用定时器中断初始化
//这里时钟选择为APB1的2倍，而APB1为36M
//arr：自动重装值。
//psc：时钟预分频数
//这里使用的是定时器3!
void TIM3_Config(void)
{
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	NVIC_InitTypeDef NVIC_InitStructure; 
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); //时钟使能
	NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;  //TIM3中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;  //先占优先级1级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;  //从优先级3级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
	
	NVIC_Init(&NVIC_InitStructure);  //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器
	TIM_TimeBaseStructure.TIM_Period = 299; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值	 计数到300为3ms
	TIM_TimeBaseStructure.TIM_Prescaler =719; //设置用来作为TIMx时钟频率除数的预分频值  100Khz的计数频率  
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
 
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);

	TIM_Cmd(TIM3, ENABLE);  //使能TIMx外设						 
}


void TIM3_IRQHandler(void)   //TIM3中断 3ms
{
	if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源 
	{
		static uint8_t cnt_3ms = 0;
		static uint8_t cnt_6ms = 0;	 
		SysTick_count++;
		cnt_3ms++;
		cnt_6ms++;		
		if(cnt_3ms == 1)  //1ms更新一次
		{
			cnt_3ms = 0;
			MpuGetData();
			RC_Analy();			
			FlightPidControl(0.003f);
			MotorControl();
		}		
		if(cnt_6ms == 2) //6ms更新一次
		{
			cnt_6ms = 0;
			GetAngle(&MPU6050,&Angle,0.00626f);
		}		
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update );  //清除TIMx的中断待处理位:TIM 中断源 
	}
}

SPI.c

#include "spi.h"

void SPI_Config(void)
{
      SPI_InitTypeDef  SPI_InitStructure;
   	  GPIO_InitTypeDef GPIO_InitStructure;

		RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2,ENABLE);  
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO  , ENABLE);
		GPIO_SetBits(GPIOB, GPIO_Pin_12); //NRF_CS预置为高 
		
	  	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15; 
	  	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	  	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 
	  	GPIO_Init(GPIOB, &GPIO_InitStructure); 

        /* SPI2 configuration */
        SPI_Cmd(SPI2, DISABLE);             //必须先禁能,才能改变MODE
        SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
        SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
        SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
        SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
        SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
        SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
        SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
        SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
        SPI_InitStructure.SPI_CRCPolynomial = 7;

        SPI_Init(SPI2, &SPI_InitStructure);

        /* SPI2 enable */
        SPI_Cmd(SPI2, ENABLE);
}
u8 SPI_RW(u8 dat)
{
        /* Loop while DR register in not emplty */
        while(SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);

        /* Send byte through the SPI2 peripheral */
        SPI_I2S_SendData(SPI2, dat);

        /* Wait to receive a byte */
        while(SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);

        /* Return the byte read from the SPI bus */
        return SPI_I2S_ReceiveData(SPI2);  
}

LED.c

#include "stm32f10x.h"
#include "LED.h"
#include "ALL_DATA.h"

sLED LED = {300,AllFlashLight};

void LEDInit(void)	
{	
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB  , ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_2|GPIO_Pin_8 | GPIO_Pin_9;		     //LED12
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
}

void LEDRefresh() //flash 300MS interval
{
	static uint32_t LastTime = 0;
	if(SysTick_count - LastTime < LED.FlashTime)
	{
		return;
	}
	else
		LastTime = SysTick_count;
	switch(LED.status)
	{
		case AlwaysOff:      //常暗   
			bLED_H();
			fLED_H();
			bLED_H();
			hLED_H();
			break;
		case AllFlashLight:				  //全部同时闪烁
			fLED_Toggle();			
			bLED_Toggle();
			hLED_Toggle();			
			aLED_Toggle();		
		  break;
		case AlwaysOn:  //常亮
		  bLED_L();
			fLED_L();
			aLED_L();
			hLED_L();
		  break;
		case AlternateFlash: //交替闪烁
			bLED_H();
			fLED_L();
			aLED_H();
			hLED_L();
			LED.status = AllFlashLight;
			break;
		case WARNING:
			fLED_L();
			hLED_L();
			bLED_Toggle();
			LED.FlashTime = 100;
			break;
		case DANGEROURS:
			bLED_L();
		  aLED_L();
			fLED_Toggle();
		  hLED_Toggle();
			LED.FlashTime = 70;
			break;
		default:
			LED.status = AlwaysOff;
			break;
	}
}

NRF24L01.c

#include "nrf24l01.h"
#include "SPI.h"
#include <string.h>

#undef SUCCESS
#define SUCCESS 0
#undef FAILED
#define FAILED  1


#define MAX_TX  		0x10  //达到最大发送次数中断
#define TX_OK   		0x20  //TX发送完成中断
#define RX_OK   		0x40  //接收到数据中断

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//24L01操作线   
#define SPI_CSN_H    (GPIOB->BSRR = GPIO_Pin_12) // PB12
#define SPI_CSN_L     (GPIOB->BRR = GPIO_Pin_12)    // PB12
#define NRF24L01_CE_H (GPIOA->BSRR = GPIO_Pin_11)    // PB1
#define NRF24L01_CE_L  (GPIOA->BRR = GPIO_Pin_11)    // PB1
#define NRF24L01_IRQ   (GPIOB->IDR&GPIO_Pin_0)//IRQ主机数据输入 PB0


#include "nrf24l01.h"
#include "SPI.h"
#include <string.h>

#undef SUCCESS
#define SUCCESS 0
#undef FAILED
#define FAILED  1
//配对密码
const uint8_t TX_ADDRESS[]= {0x11,0x22,0x33,0x44,0x55};	//本地地址
const uint8_t RX_ADDRESS[]= {0x11,0x22,0x33,0x44,0x55};	//接收地址RX_ADDR_P0 == RX_ADDR

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//24L01操作线   
#define Set_NRF24L01_CSN    (GPIOB->BSRR = GPIO_Pin_12) // PB12
#define Clr_NRF24L01_CSN     (GPIOB->BRR = GPIO_Pin_12)    // PB12
#define Set_NRF24L01_CE (GPIOA->BSRR = GPIO_Pin_11)    // PB1
#define Clr_NRF24L01_CE  (GPIOA->BRR = GPIO_Pin_11)    // PB1
#define READ_NRF24L01_IRQ   (GPIOA->IDR&GPIO_Pin_8)//IRQ主机数据输入 PB0

//初始化24L01的IO口
void NRF24L01_Configuration(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);    //使能GPIO的时钟  CE
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;          //NRF24L01  
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;    //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);   //使能GPIO的时钟 CSN
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;      
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;    //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	Set_NRF24L01_CE;                                    //初始化时先拉高
	Set_NRF24L01_CSN;                                   //初始化时先拉高

    //配置NRF2401的IRQ
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU  ;     //上拉输入
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	GPIO_SetBits(GPIOA,GPIO_Pin_8);

	SPI_Config();                                //初始化SPI
	Clr_NRF24L01_CE; 	                                //使能24L01
	Set_NRF24L01_CSN;                                   //SPI片选取消
}
//上电检测NRF24L01是否在位
//写5个数据然后再读回来进行比较，
//相同时返回值:0，表示在位;否则返回1，表示不在位	
u8 NRF24L01_Check(void)
{
	u8 buf[5]={0XA5,0XA5,0XA5,0XA5,0XA5};
	u8 buf1[5];
	u8 i;   	 
	NRF24L01_Write_Buf(SPI_WRITE_REG+TX_ADDR,buf,5);//写入5个字节的地址.	
	NRF24L01_Read_Buf(TX_ADDR,buf1,5);              //读出写入的地址  	
	for(i=0;i<5;i++)if(buf1[i]!=0XA5)break;					   
	if(i!=5)return 1;                               //NRF24L01不在位	
	return 0;		                                //NRF24L01在位
}	 	 
//通过SPI写寄存器
u8 NRF24L01_Write_Reg(u8 regaddr,u8 data)
{
	u8 status;	
    Clr_NRF24L01_CSN;                    //使能SPI传输
  	status =SPI_RW(regaddr); //发送寄存器号 
  	SPI_RW(data);            //写入寄存器的值
  	Set_NRF24L01_CSN;                    //禁止SPI传输	   
  	return(status);       		         //返回状态值
}
//读取SPI寄存器值 ，regaddr:要读的寄存器
u8 NRF24L01_Read_Reg(u8 regaddr)
{
	u8 reg_val;	    
	Clr_NRF24L01_CSN;                //使能SPI传输		
  	SPI_RW(regaddr);     //发送寄存器号
  	reg_val=SPI_RW(0XFF);//读取寄存器内容
  	Set_NRF24L01_CSN;                //禁止SPI传输		    
  	return(reg_val);                 //返回状态值
}	
//在指定位置读出指定长度的数据
//*pBuf:数据指针
//返回值,此次读到的状态寄存器值 
u8 NRF24L01_Read_Buf(u8 regaddr,u8 *pBuf,u8 datalen)
{
	u8 status,u8_ctr;	       
  	Clr_NRF24L01_CSN;                     //使能SPI传输
  	status=SPI_RW(regaddr);   //发送寄存器值(位置),并读取状态值   	   
	for(u8_ctr=0;u8_ctr<datalen;u8_ctr++)pBuf[u8_ctr]=SPI_RW(0XFF);//读出数据
  	Set_NRF24L01_CSN;                     //关闭SPI传输
  	return status;                        //返回读到的状态值
}
//在指定位置写指定长度的数据
//*pBuf:数据指针
//返回值,此次读到的状态寄存器值
u8 NRF24L01_Write_Buf(u8 regaddr, u8 *pBuf, u8 datalen)
{
	u8 status,u8_ctr;	    
	Clr_NRF24L01_CSN;                                    //使能SPI传输
  	status = SPI_RW(regaddr);                //发送寄存器值(位置),并读取状态值
  	for(u8_ctr=0; u8_ctr<datalen; u8_ctr++)SPI_RW(*pBuf++); //写入数据	 
  	Set_NRF24L01_CSN;                                    //关闭SPI传输
  	return status;                                       //返回读到的状态值
}				   
//启动NRF24L01发送一次数据
//txbuf:待发送数据首地址
//返回值:发送完成状况
u8 NRF24L01_TxPacket(u8 *txbuf)
{
	u8 state;   
	Clr_NRF24L01_CE;
	NRF24L01_Write_Buf(WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);//写数据到TX BUF  32个字节
	Set_NRF24L01_CE;                                     //启动发送	   
	while(READ_NRF24L01_IRQ!=0);                         //等待发送完成
	state=NRF24L01_Read_Reg(STATUS);                     //读取状态寄存器的值	   
	NRF24L01_Write_Reg(SPI_WRITE_REG+STATUS,state);      //清除TX_DS或MAX_RT中断标志
	if(state&MAX_TX)                                     //达到最大重发次数
	{
		NRF24L01_Write_Reg(FLUSH_TX,0xff);               //清除TX FIFO寄存器 
		return MAX_TX; 
	}
	if(state&TX_OK)                                      //发送完成
	{
		return SUCCESS;
	}
	return FAILED;                                         //其他原因发送失败
}

//启动NRF24L01发送一次数据
//txbuf:待发送数据首地址
//返回值:0，接收完成；其他，错误代码
u8 NRF24L01_RxPacket(u8 *rxbuf)
{
	u8 state;		    							      
	state=NRF24L01_Read_Reg(STATUS);                //读取状态寄存器的值    	 
	NRF24L01_Write_Reg(SPI_WRITE_REG+STATUS,state); //清除TX_DS或MAX_RT中断标志
	if(state&RX_OK)                                 //接收到数据
	{
		NRF24L01_Read_Buf(RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);//读取数据
		NRF24L01_Write_Reg(FLUSH_RX,0xff);          //清除RX FIFO寄存器 
		return SUCCESS; 
	}	   
	return FAILED;                                      //没收到任何数据
}

//该函数初始化NRF24L01到RX模式
//设置RX地址,写RX数据宽度,选择RF频道,波特率和LNA HCURR
//当CE变高后,即进入RX模式,并可以接收数据了		   
void RX_Mode(void)
{
	Clr_NRF24L01_CE;	  
    //写RX节点地址
  	NRF24L01_Write_Buf(SPI_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH);

    //使能通道0的自动应答    
  	NRF24L01_Write_Reg(SPI_WRITE_REG+EN_AA,0x01);    
    //使能通道0的接收地址  	 
  	NRF24L01_Write_Reg(SPI_WRITE_REG+EN_RXADDR,0x01); 
	  //设置RF通信频率		  
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RF_CH,45);	  //(要改单机控制，就把45改成跟遥控器单独一样的。就可以单机控制了)
    //选择通道0的有效数据宽度 	    
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);
    //设置TX发射参数,0db增益,2Mbps,低噪声增益开启   
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RF_SETUP,0x0f);
    //配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,PRIM_RX接收模式 
  	NRF24L01_Write_Reg(SPI_WRITE_REG+NCONFIG, 0x0f); 
    //CE为高,进入接收模式 
  	Set_NRF24L01_CE;                                
}			

//该函数初始化NRF24L01到TX模式
//设置TX地址,写TX数据宽度,设置RX自动应答的地址,填充TX发送数据,
//选择RF频道,波特率和LNA HCURR PWR_UP,CRC使能
//当CE变高后,即进入RX模式,并可以接收数据了		   
//CE为高大于10us,则启动发送.	 
void TX_Mode(void)
{														 
	Clr_NRF24L01_CE;	    
    //写TX节点地址 
  	NRF24L01_Write_Buf(SPI_WRITE_REG+TX_ADDR,(u8*)TX_ADDRESS,TX_ADR_WIDTH);    
    //设置TX节点地址,主要为了使能ACK	  
  	NRF24L01_Write_Buf(SPI_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH); 

    //使能通道0的自动应答    
  	NRF24L01_Write_Reg(SPI_WRITE_REG+EN_AA,0x01);     
    //使能通道0的接收地址  
  	NRF24L01_Write_Reg(SPI_WRITE_REG+EN_RXADDR,0x01); 
    //设置自动重发间隔时间:500us + 86us;最大自动重发次数:10次
  	NRF24L01_Write_Reg(SPI_WRITE_REG+SETUP_RETR,0x1a);
    //设置RF通道为40
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RF_CH,45);       //(要改单机控制，就把45改成跟遥控器单独一样的。就可以单机控制了)
    //设置TX发射参数,0db增益,2Mbps,低噪声增益开启   
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RF_SETUP,0x0f);  //0x27  250K   0x07 1M
    //配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,PRIM_RX发送模式,开启所有中断
  	NRF24L01_Write_Reg(SPI_WRITE_REG+NCONFIG,0x0e);    
    // CE为高,10us后启动发送
	Set_NRF24L01_CE;                                  
}		  

void NRF24L01_init(void)
{
	NRF24L01_Configuration();
  do
	{
    RX_Mode();
	}while(NRF24L01_Check() == FAILED);
}

MPU6050.c

#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"



#define	SMPLRT_DIV		0x19	//陀螺仪采样率，典型值：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 MPU6052C_PRODUCT_ID 0x72

//#define   MPU6050_is_DRY()      GPIO_ReadOutBit(HT_GPIOC, GPIO_PIN_0)//IRQ主机数据输入
	#ifdef  	USE_I2C_HARDWARE
		
		#define MPU6050_ADDRESS 0xD0//0x68
	#else
		#define  MPU6050_ADDRESS 0xD0   //IIC写入时的地址字节数据，+1为读取
	#endif



int16_t MpuOffset[6] = {0};

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

int8_t mpu6050_rest(void)
{
	if(IIC_Write_One_Byte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80) == FAILED)
		return FAILED;	//复位
	delay_ms(20);
	return SUCCESS;
}
/****************************************************************************************
*@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); //陀螺仪采样率，0x00(500Hz)
	date += IIC_Write_One_Byte(MPU6050_ADDRESS, PWR_MGMT_1, 0x03);	//设置设备时钟源，陀螺仪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);
	if(date!= MPU6050_PRODUCT_ID)
		return FAILED;
	else 
		MpuGetOffset();
		return SUCCESS;
}


#define  Gyro_Read() IIC_read_Bytes(MPU6050_ADDRESS, 0X3B,buffer,6)
#define  Acc_Read() IIC_read_Bytes(MPU6050_ADDRESS, 0x43,&buffer[6],6)

void MpuGetData(void) //读取陀螺仪数据加滤波
{
	  uint8_t i;
    uint8_t buffer[12];

		Gyro_Read();
	  Acc_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,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,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.15f;  //滤波因素			
			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;   //set offset from the 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++)
	{		
		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  //使能
		);
}

ANTO.c

#include <stdlib.h>
#include <string.h>
#include "ALL_DATA.h"
#include "ANO_DT.h"
#include "USART.h"

static uint8_t RatePID[18];
static uint8_t AnglePID[18];
static uint8_t HighPID[18];

static struct{
	uint8_t PID1 :1; //接受到上位机PID组1
	uint8_t PID2 :1; //接受到上位机PID组2
	uint8_t PID3 :1; //接受到上位机PID组3
	uint8_t PID4 :1; //接受到上位机PID组4
	uint8_t PID5 :1; //接受到上位机PID组5
	uint8_t PID6 :1; //接受到上位机PID组6	
	uint8_t CMD2_READ_PID:1; //接受到上位机读取PID的请求
}ANTO_Recived_flag;

enum                                  //上位请求飞控类型
{
	READ_PID = 0X01,                    //读取飞控的PID请求
	READ_MODE = 0x02,                   //读取飞行模式
	READ_ROUTE = 0x21,                  //读取航点信息
	READ_VERSION = 0XA0,                //读取飞控版本
	RETURN_DEFAULT_PID = 0xA1           //恢复默认PID
 }CMD2;

void ANO_Recive(int8_t *pt)                   //接收到上位机的数据
{
	switch(pt[2])
	{
		case ANTO_RATE_PID:
			ANTO_Recived_flag.PID1 = 1;             //接收到上位机发来的PID数据
			memcpy(RatePID,&pt[4],18);              //先把接收到的数据提出来，防止被下一组PID数据覆盖，这组的PID是给速度环用的
			break;
		case ANTO_ANGLE_PID:                      //这组的PID是给角度环用的
			memcpy(AnglePID,&pt[4],18);
			ANTO_Recived_flag.PID2 = 1;
			break;
		case ANTO_HEIGHT_PID:                     //这组的PID是给高度环用的
			memcpy(HighPID,&pt[4],18);
			ANTO_Recived_flag.PID3 = 1;
			break;
		case ANTO_PID4:
			break;
		case ANTO_PID5:   
			break;
		case ANTO_PID6:
			break;
		case 0x01:                                //上位机发来的CMD1 包含各种校准
			break;
		case 0x02:                                //上位机发来的CMD2 包含请求读取PID等
			{
				switch(*(uint8_t*)&pt[4])             //判断上位机发来CMD的内容
				{
					case READ_PID:                      //上位机请求读取飞控PID数据
						ANTO_Recived_flag.CMD2_READ_PID = 1;
						break;
					case READ_MODE: 
						break;
					case READ_ROUTE: 
						break;					
					case READ_VERSION:  
						break;
					case RETURN_DEFAULT_PID:  
						break;					
					default: 
						break;					
				}			
			}
			break;
		case ANTO_RCDATA: //Immediately deal with 
			break;
		default:
			break;			
	}
	return;
}

static void ANTO_Send(const enum ANTO_SEND FUNCTION) //发送数据到上位机
{
	uint8_t i;
	uint8_t len=2;
	int16_t Anto[12];
	int8_t *pt = (int8_t*)(Anto);
	PidObject *pidX=0;
	PidObject *pidY=0;
	PidObject *pidZ=0;

	switch(FUNCTION)
	{
		case ANTO_RATE_PID:      //PID1
				 pidX = &pidRateX;
				 pidY = &pidRateY;
				 pidZ = &pidRateZ;
         goto send_pid;		
		case ANTO_ANGLE_PID:      //PID2
				 pidX = &pidRoll;
				 pidY = &pidPitch;
				 pidZ = &pidYaw;
				 goto send_pid;				
		case ANTO_HEIGHT_PID:     //PID3
				 pidX = &pidHeightRate;
				 pidY = &pidHeightHigh;
				 goto send_pid;							
		case ANTO_PID4:	  //PID4
		case ANTO_PID5:	         //PID5
    case ANTO_PID6:
send_pid:
			if(pidX!=NULL)
			{
				Anto[2] = (int16_t)(pidX->kp *1000);
				Anto[3] = (int16_t)(pidX->ki *1000);
				Anto[4] = (int16_t)(pidX->kd *1000);
			}
			if(pidY!=NULL)
			{
				Anto[5] = (int16_t)(pidY->kp *1000);
				Anto[6] = (int16_t)(pidY->ki *1000);
				Anto[7] = (int16_t)(pidY->kd *1000);
			}
			if(pidZ!=NULL)
			{
				Anto[8] = (int16_t)(pidZ->kp *1000);
				Anto[9] = (int16_t)(pidZ->ki *1000);
				Anto[10] = (int16_t)(pidZ->kd *1000);
			}
			len = 18;
			break;
		case ANTO_MOTOR:    //send motor

				len = 8;
			break;	
		case ANTO_RCDATA: //send RC data

			break;
		case ANTO_MPU_MAGIC:     //发送MPU6050和磁力计的数据
			memcpy(&Anto[2],(int8_t*)&MPU6050,sizeof(_st_Mpu));
			memcpy(&Anto[8],(int8_t*)&AK8975,sizeof(_st_Mag));
			len = 18;
			break;
		case ANTO_SENSER2:

			break;
		case ANTO_STATUS:     //send angle
			
				Anto[2] = (int16_t)(-Angle.roll*100);
				Anto[3] = (int16_t)(Angle.pitch*100);
				Anto[4] = (int16_t)(-Angle.yaw*100);
	   
				len = 6;
			break;
		case ANTO_POWER:

				break;
		default:
			break;			
	}
	
	Anto[0] = 0XAAAA;
	Anto[1] = len | FUNCTION<<8;
	pt[len+4] = (int8_t)(0xAA+0xAA);
	for(i=2;i<len+4;i+=2)    //a swap with b;
	{
		pt[i] ^= pt[i+1];
		pt[i+1] ^= pt[i];
		pt[i] ^= pt[i+1];
		pt[len+4] += pt[i] + pt[i+1];
	}
	
	USART3_SendByte(pt,len+5);
}

void ANTO_polling(void) //轮询扫描上位机端口
{
	volatile static uint8_t status = 0;
	switch(status)
	{
		case 0:
			
			status = 1;
			break;
		case 1:
			ANTO_Send(ANTO_MPU_MAGIC);
			delay_ms(1);
	  	ANTO_Send(ANTO_STATUS);
			delay_ms(1);

			if(*(uint8_t*)&ANTO_Recived_flag != 0) //一旦接收到上位机的数据，则暂停发送数据到上位机，转而去判断上位机要求飞控做什么。
			{
				status = 2;
			}
		 	break;
		case 2:
			if(*(uint8_t*)&ANTO_Recived_flag == 0)//上位机的发过来的数据都被处理了，则返回专心的发送数据到上位机
			{
				status = 1;
			}
	
			if(ANTO_Recived_flag.CMD2_READ_PID) //判断上位机是否请求发发送飞控PID数据到上位机
			{		
					ANTO_Send(ANTO_RATE_PID);
					delay_ms(1);
					ANTO_Send(ANTO_ANGLE_PID);
					delay_ms(1);
					ANTO_Send(ANTO_HEIGHT_PID);
					delay_ms(1);
					ANTO_Recived_flag.CMD2_READ_PID = 0;
			}
			
			if(*(uint8_t*)&ANTO_Recived_flag & 0x3f) //接收到上位机发来的PID数据
			{
					PidObject *pidX=0;
					PidObject *pidY=0;
					PidObject *pidZ=0;
				  uint8_t *P;
				
					if(ANTO_Recived_flag.PID1)
					{
						 pidX = &pidRateX;
						 pidY = &pidRateY;
						 pidZ = &pidRateZ;
						 P = RatePID;
						 ANTO_Recived_flag.PID1 = 0;
					}
					else if(ANTO_Recived_flag.PID2)
					{
						 pidX = &pidRoll;
						 pidY = &pidPitch;
						 pidZ = &pidYaw;
						 P = AnglePID;	
						 ANTO_Recived_flag.PID2 = 0;                             
					}
					else if(ANTO_Recived_flag.PID3)
					{
						 pidX = &pidHeightRate;
						 pidY = &pidHeightHigh;
						 P = HighPID;	
						 ANTO_Recived_flag.PID3 = 0;     						
					}
					else
					{
						break;
					}
					{
							union {
								uint16_t _16;
								uint8_t _u8[2];
							}data;
							
							if(pidX!=NULL)
							{
								data._u8[1] = P[0]; 
								data._u8[0] = P[1];
								pidX->kp =  data._16 /1000.0f;
								data._u8[1] = P[2]; 
								data._u8[0] = P[3];
								pidX->ki =  data._16 /1000.0f;
								data._u8[1] = P[4]; 
								data._u8[0] = P[5];
								pidX->kd =  data._16 /1000.0f;								
							}
							if(pidY!=NULL)
							{
								data._u8[1] = P[6]; 
								data._u8[0] = P[7];
								pidY->kp =  data._16 /1000.0f;
								data._u8[1] = P[8]; 
								data._u8[0] = P[9];
								pidY->ki =  data._16 /1000.0f;
								data._u8[1] = P[10]; 
								data._u8[0] = P[11];
								pidY->kd =  data._16 /1000.0f;		
							}
							if(pidZ!=NULL)
							{
								data._u8[1] = P[12]; 
								data._u8[0] = P[13];
								pidZ->kp =  data._16 /1000.0f;
								data._u8[1] = P[14]; 
								data._u8[0] = P[15];
								pidZ->ki =  data._16 /1000.0f;
								data._u8[1] = P[16]; 
								data._u8[0] = P[17];
								pidZ->kd =  data._16 /1000.0f;		
							}				
					}				
			}
			break;
		default:
			break;
	}

}

void USART3_IRQHandler(void) //串口接收
{ 	
  static  int8_t ReciveBuffer[25];
  static uint8_t count;
	if ((USART3->SR & USART_IT_ORE))//是否接收寄存器溢出
	{
	
	}	
  if((USART3->SR & USART_IT_RXNE))
  {
		ReciveBuffer[count] = USART3->DR;	
		switch(count)
		{
		case 0:
			if(ReciveBuffer[0]==(int8_t)0xAA)
				count++;
				break;
		case 1:	
			if(ReciveBuffer[1]==(int8_t)0xAF)
					count++;
			else 
					count = 0;
				break;
		default:
			if(count < ReciveBuffer[3]+4)
			{
				count++;
				break;
			}
			else
			{
				uint8_t i;
				int8_t CheckSum=0;
							
				for(i=0;i<count;i++)
				{
					CheckSum += ReciveBuffer[i];			
				}		
				if(CheckSum == ReciveBuffer[count])  //if the data calculate sum equal to the  final data from PC.
				{
						static int8_t CheckSend[7]={0xAA,0XAA,0xEF,2,0,0,0};	
						
						CheckSend[4] = ReciveBuffer[2];
						CheckSend[5] = CheckSum;
						CheckSend[6] = 0;
						for(i=0;i<6;i++)
						{
							CheckSend[6] += CheckSend[i]; 					
						}
						USART3_SendByte(CheckSend,7);
						ANO_Recive(ReciveBuffer);			//To arrange the data	and give the result to control argument.	
				}			
				count = 0;                  //return to the first data point,and retore from the head buffer next time.
				ReciveBuffer[0] = 0;  //reset the data buffer.
				ReciveBuffer[1] = 0;
			}
			break;							
		}
	}
}

Remote.c

#include "ALL_DATA.h"
#include "nrf24l01.h"
#include "control.h"
#include <math.h>
#include "myMath.h"
#include "LED.h"
#include "Remote.h"

#define SUCCESS 0
#undef FAILED
#define FAILED  1

/*****************************************************************************************
 *  通道数据处理    
 ******************************************************************************************/	
uint8_t RC_rxData[32];
void remote_unlock(void);	
void RC_Analy(void)  
{
	static uint16_t cnt;
	const float roll_pitch_ratio = 0.04f;
	if(NRF24L01_RxPacket(RC_rxData)==SUCCESS)
	{ 	

		uint8_t i;
		uint8_t CheckSum=0;
		cnt = 0;
		for(i=0;i<31;i++)
		{
			CheckSum +=  RC_rxData[i];
		}
		if(RC_rxData[31]==CheckSum && RC_rxData[0]==0xAA && RC_rxData[1]==0xAF)  //如果接收到的遥控数据正确
		{
			Remote.roll = ((uint16_t)RC_rxData[4]<<8) | RC_rxData[5];  //通道1
			Remote.roll =Remote.roll < 1000  ? 1000 : ( Remote.roll > 2000? 2000 : Remote.roll );//roll限幅
			Remote.pitch = ((uint16_t)RC_rxData[6]<<8) | RC_rxData[7];  //通道2
			Remote.pitch =Remote.pitch < 1000  ? 1000 : ( Remote.pitch > 2000? 2000 : Remote.pitch );
			Remote.thr = 	((uint16_t)RC_rxData[8]<<8) | RC_rxData[9];   //通道3
			Remote.thr =Remote.thr < 1000  ? 1000 : ( Remote.thr > 2000? 2000 : Remote.thr );//油门限幅
			Remote.yaw =  ((uint16_t)RC_rxData[10]<<8) | RC_rxData[11];   //通道4
			Remote.yaw =Remote.thr < 1000  ? 1000 : ( Remote.yaw > 2000? 2000 : Remote.yaw );
			Remote.AUX1 =  ((uint16_t)RC_rxData[12]<<8) | RC_rxData[13];   //通道5  左上角按键都属于通道5  
			Remote.AUX1 =Remote.AUX1 < 1000  ? 1000 : ( Remote.AUX1 > 2000? 2000 : Remote.AUX1 );
			Remote.AUX2 =  ((uint16_t)RC_rxData[14]<<8) | RC_rxData[15];   //通道6  右上角按键都属于通道6 
			Remote.AUX2 =Remote.AUX2 < 1000  ? 1000 : ( Remote.AUX2 > 2000? 2000 : Remote.AUX2 );
			Remote.AUX3 =  ((uint16_t)RC_rxData[16]<<8) | RC_rxData[17];   //通道7  左下边按键都属于通道7 
			Remote.AUX3 =Remote.AUX3 < 1000  ? 1000 : ( Remote.AUX3 > 2000? 2000 : Remote.AUX3 );
			Remote.AUX4 =  ((uint16_t)RC_rxData[18]<<8) | RC_rxData[19];   //通道8  右下边按键都属于通道6  
			Remote.AUX4 =Remote.AUX4 < 1000  ? 1000 : ( Remote.AUX4 > 2000? 2000 : Remote.AUX4 );
				
			pidPitch.desired =-(Remote.pitch-1500)*roll_pitch_ratio;	 //将遥杆值作为飞行角度的期望值
			pidRoll.desired = -(Remote.roll-1500)*roll_pitch_ratio;
			if(Remote.yaw>1820)
			{
				pidYaw.desired += 0.75f;	
			}
			else if(Remote.yaw <1180)
			{
				pidYaw.desired -= 0.75f;	
			}						
			remote_unlock();
		}
  }
//如果3秒没收到遥控数据，则判断遥控信号丢失，飞控在任何时候停止飞行，避免伤人。
	else
	{		
		cnt++;
		if(cnt>800)
		{
			cnt = 0;
			ALL_flag.unlock = 0; 
			NRF24L01_init();
		}
	}
}

/*****************************************************************************************
 *  解锁判断
 ******************************************************************************************/	
void remote_unlock(void)
{
	volatile static uint8_t status=WAITING_1;
	static uint16_t cnt=0;
	if(Remote.thr<1200 &&Remote.yaw<1200)                         //油门遥杆左下角锁定
	{
		status = EXIT_255;
	}	
	switch(status)
	{
		case WAITING_1://等待解锁
			if(Remote.thr<1150)           //解锁三步，油门最低->油门最高->油门最低 看到LED灯不闪了 即完成解锁
			{			 
				status = WAITING_2;				 
			}		
			break;
		case WAITING_2:
			if(Remote.thr>1800)          
			{		
				static uint8_t cnt = 0;
				cnt++;		
				if(cnt>5) //最高油门需保持200ms以上，防止遥控开机初始化未完成的错误数据
				{	
						cnt=0;
						status = WAITING_3;
				}
			}			
			break;
		case WAITING_3:
			if(Remote.thr<1150)          
			{			 
				status = WAITING_4;				 
			}			
			break;			
		case WAITING_4:	//解锁前准备	               
			ALL_flag.unlock = 1;
			status = PROCESS_31;
			LED.status = AlwaysOn;									
			break;		
		case PROCESS_31:	//进入解锁状态
			if(Remote.thr<1020)
			{
				if(cnt++ > 2000)                                     // 油门遥杆处于最低6S自动上锁
				{								
					status = EXIT_255;								
				}
			}
			else if(!ALL_flag.unlock)                           //Other conditions lock 
			{
				status = EXIT_255;				
			}
			else					
				cnt = 0;
			break;
		case EXIT_255: //进入锁定
			LED.status = AllFlashLight;	                                 //exit
			cnt = 0;
			LED.FlashTime = 300; //100*3ms		
			ALL_flag.unlock = 0;
			status = WAITING_1;
			break;
		default:
			status = EXIT_255;
			break;
	}
}

Control.c

#include "ALL_DATA.h" 
#include "ALL_DEFINE.h" 
#include "control.h"
#include "pid.h"

#undef NULL
#define NULL 0
#undef DISABLE 
#define DISABLE 0
#undef ENABLE 
#define ENABLE 1
#undef REST
#define REST 0
#undef SET 
#define SET 1 
#undef EMERGENT
#define EMERGENT 0

#define MOTOR1 motor[0] 
#define MOTOR2 motor[1] 
#define MOTOR3 motor[2] 
#define MOTOR4 motor[3] 

PidObject *(pPidObject[])={&pidRateX,&pidRateY,&pidRateZ,&pidRoll,&pidPitch,&pidYaw   //结构体数组，将每一个数组放一个pid结构体，这样就可以批量操作各个PID的数据了  比如解锁时批量复位pid控制数据，新手明白这句话的作用就可以了
		,&pidHeightRate
		,&pidHeightHigh
};

void FlightPidControl(float dt)
{
	volatile static uint8_t status=WAITING_1;

	switch(status)
	{		
		case WAITING_1: //等待解锁
			if(ALL_flag.unlock)
			{
				status = READY_11;	
			}			
			break;
		case READY_11:  //准备进入控制
			pidRest(pPidObject,8); //批量复位PID数据，防止上次遗留的数据影响本次控制

			Angle.yaw = pidYaw.desired =  pidYaw.measured = 0;   //锁定偏航角
		
			status = PROCESS_31;
		
			break;			
		case PROCESS_31: //正式进入控制
			
      pidRateX.measured = MPU6050.gyroX * Gyro_G; //内环测量值 角度/秒
			pidRateY.measured = MPU6050.gyroY * Gyro_G;
			pidRateZ.measured = MPU6050.gyroZ * Gyro_G;
		
			pidPitch.measured = Angle.pitch; //外环测量值 单位：角度
		  pidRoll.measured = Angle.roll;
			pidYaw.measured = Angle.yaw;
		
		 	pidUpdate(&pidRoll,dt);    //调用PID处理函数来处理外环	横滚角PID		
			pidRateX.desired = pidRoll.out; //将外环的PID输出作为内环PID的期望值即为串级PID
			pidUpdate(&pidRateX,dt);  //再调用内环

		 	pidUpdate(&pidPitch,dt);    //调用PID处理函数来处理外环	俯仰角PID	
			pidRateY.desired = pidPitch.out;  
			pidUpdate(&pidRateY,dt); //再调用内环

			CascadePID(&pidRateZ,&pidYaw,dt);	//也可以直接调用串级PID函数来处理
			break;
		case EXIT_255:  //退出控制
			pidRest(pPidObject,8);
			status = WAITING_1;//返回等待解锁
		  break;
		default:
			status = EXIT_255;
			break;
	}
	if(ALL_flag.unlock == EMERGENT) //意外情况，请使用遥控紧急上锁，飞控就可以在任何情况下紧急中止飞行，锁定飞行器，退出PID控制
		status = EXIT_255;
}


int16_t motor[4];


void MotorControl(void)
{	
	volatile static uint8_t status=WAITING_1;
	
	
	if(ALL_flag.unlock == EMERGENT) //意外情况，请使用遥控紧急上锁，飞控就可以在任何情况下紧急中止飞行，锁定飞行器，退出PID控制
		status = EXIT_255;	
	switch(status)
	{		
		case WAITING_1: //等待解锁	
			MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = 0;  //如果锁定，则电机输出都为0
			if(ALL_flag.unlock)
			{
				status = WAITING_2;
			}
		case WAITING_2: //解锁完成后判断使用者是否开始拨动遥杆进行飞行控制
			if(Remote.thr>1100)
			{
				status = PROCESS_31;
			}
			break;
		case PROCESS_31:
			{
				int16_t temp;
				temp = Remote.thr -1000 + pidHeightRate.out; //油门+定高输出值
				if(Remote.thr<1020)		//油门太低了，则限制输出  不然飞机乱转												
				{
					MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4=0;
					break;
				}
				MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = LIMIT(temp,0,900); //留100给姿态控制

				MOTOR1 +=    + pidRateX.out - pidRateY.out - pidRateZ.out;//; 姿态输出分配给各个电机的控制量
				MOTOR2 +=    + pidRateX.out + pidRateY.out + pidRateZ.out ;//;
				MOTOR3 +=    - pidRateX.out + pidRateY.out - pidRateZ.out;
				MOTOR4 +=    - pidRateX.out - pidRateY.out + pidRateZ.out;//;
			}	
			break;
		case EXIT_255:
			MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = 0;  //如果锁定，则电机输出都为0
			status = WAITING_1;	
			break;
		default:
			break;
	}	
	TIM2->CCR1 = LIMIT(MOTOR1,0,1000);  //更新PWM
	TIM2->CCR2 = LIMIT(MOTOR2,0,1000);
	TIM2->CCR3 = LIMIT(MOTOR3,0,1000);
	TIM2->CCR4 = LIMIT(MOTOR4,0,1000);
} 

kalman.c

#include "kalman.h"
#include <math.h>
#include "delay.h"
/*********************************************************************************************************************
**卡尔曼滤波
**@brief: 线性最优评估滤波
**@param[in]  InputData 滤波前的数据，QR误差
**@param[out] None
**@return 滤波后的数据
**@remark: 通过修改过程噪声和测量噪声R,Q值优化输出值
X(k)=A X(k-1)+B U(k)+W(k)  
Z(k)=H X(k)+V(k)
AB是系统参数
XK时刻的系统状态
H：测量系统的参数
Z：K时刻的测量值
WV：过程和测量噪声

X(k|k-1)=X(k-1|k-1) 预测下一状态的系统
P(k|k-1)=P(k-1|k-1) +Q  
Kg(k)= P(k|k-1) / (P(k|k-1) + R)   计算Kg卡尔曼增益
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1))   根据预测值结合估算值得出当前状态值
P(k|k)=(1-Kg(k))P(k|k-1)  更新协方差


(k-1|k-1)上一个状态的最优评估
(k|k-1) 由上一个状态的最优评估预测当前状态的最优评估
(k|k)  由预测本状态的评估具体实现最优评估

Q:系统过程的协方差
R：测量的协方差
高斯白 = Q+R
Kg：卡尔曼增益
P：协方差

注:本卡尔曼滤波器争对单模型，单测量H,I均为1，没有控制量U=0，通常对A,B初始值取1
注：X会逐渐收敛，X(0|0)初始测量值状态根据，测量前的数值而定。
注 :   P (0|0)一般不取0，取0意味在0时候的方差为0，系统认为0时刻的值是最优的。然而在实际系统中往往0时刻不是最优的
 **********************************************************************************************************************/
 		//		const float Q = 0.018;//0.01; 
		//		const float R = 0.543;//0.9;			
//float kalman_1(float InputData,float Q,float R)  //一维卡尔曼
//{
//	struct Kalman{
//		float K_1_K_1;//(k-1|k-1)
//		float K_K_1;  //(k|k-1)
//		float K_K;    //(k|k)
//	};
//	static struct Kalman P={0};
//	static struct Kalman X={0};
//	float Kg;
//	
//	X.K_K_1 = X.K_1_K_1;
//	P.K_K_1 = P.K_1_K_1 + Q;
//	Kg = P.K_K_1 / (P.K_K_1 + R);
//	X.K_K = X.K_K_1 + Kg * (InputData - X.K_K_1);
//	P.K_K = (1-Kg) * P.K_K_1 ;
//	
//	X.K_1_K_1 = X.K_K;
//	P.K_1_K_1 = P.K_K;
//	return X.K_K;
//}

void kalman_1(struct _1_ekf_filter *ekf,float input)  //一维卡尔曼
{
	ekf->Now_P = ekf->LastP + ekf->Q;
	ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
	ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
	ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
}


/*****************************************************
*多维卡尔曼是一维卡尔曼的多维噪声扩展。
*利用角速度估计角度的预测值，同时角度更新角速度的偏置补偿值。
*http://wapwenku.baidu.com/view/0ac55e8cf01dc281e53af0ff?pn=5&pu=
*/
float kalman_2_Update(float InputAngle,float InputGyro,float dt)  //二维卡尔曼
{
	struct Kalman{
		float k_1_k_1;//(k-1|k-1)
		float k_k_1;  //(k|k-1)
		float k_k;    //(k|k)
	};
	static struct Kalman X={0};
		
	const float R_angle=0.5; //角度计算的过程噪声		
	const float h_0=1; //二维卡尔曼矩阵系数
	const float Q_angle=0.001;  
	const float Q_gyro=0.001;	
		
	static float P[2][2];
	float k_0,k_1;
	float t0,t1;
	float PHt_0;
	float PHt_1;
	float Pdot[4];	
	float E;
//矩阵	
//A=[1,dt]
//	[0,1 ];
//B=[dt,0];
//H=[1,0];
//P=[a,b]
	//[c,d];
//事实上X输入的也是二维矩阵X[InputAngle,InputGyro]，而初始值即为X.k_1_k_1和Q_bias，可以取0
//--------------------------------		
//X(k|k-1)=A X(k-1|k-1)+B U(k) 	
	static float Q_bias; //角速度偏置补偿值
	X.k_k_1 = X.k_1_k_1 + (InputGyro - Q_bias) * dt; //用角速度积分预测角度新值
//---------------------------------		
// P(k|k-1)=A P(k-1|k-1) A'

	Pdot[0]=Q_angle - P[0][1] - P[1][0]; 

	Pdot[1]= - P[1][1];

	Pdot[2]= - P[1][1];

	Pdot[3]= Q_gyro;

	P[0][0] += Pdot[0] * dt;

	P[0][1] += Pdot[1] * dt;

	P[1][0] += Pdot[2] * dt;

	P[1][1] += Pdot[3] * dt;
	//以上各式由矩阵运算得来
//---------------------------------	
/*
* Update covariance matrix:
* (equation 21-3)
*
* P = P - K H P
* Let
* Y = H P

*/
//Kg(k)= P(k|k-1) H'/ (H P(k|k-1) H' + R) 
	PHt_0 = h_0 * P[0][0];

	PHt_1 = h_0 * P[1][0];

	E = R_angle + h_0 * PHt_0;

	k_0 = PHt_0 / E;

	k_1 = PHt_1 / E;
//---------------------------------	
//P(k|k)=(I-Kg(k) H)P(k|k-1)	

	t0 = PHt_0;

	t1 = h_0 * P[0][1];

	P[0][0] -= k_0 * t0;

	P[0][1] -= k_0 * t1;

	P[1][0] -= k_1 * t0;

	P[1][1] -= k_1 * t1;

//---------------------------------	
	
/*
* Update our state estimate: *

* Xnew = X + K * error

* X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) 
*/

	X.k_k = X.k_k_1 + k_0 * (InputAngle - X.k_k_1);

	Q_bias = Q_bias + k_1 * (InputAngle - X.k_k_1);
	

	X.k_1_k_1 = X.k_k; //为下一次计算做准备
	
	return X.k_k;
}
//对比一维二维的性质，可以扩展N维卡尔曼
/*********************************************END OF FILE ***********************************************************/
/*********************************************************************************************************/
/*********************************************************************************************************/
/*********************************************************************************************************/
/*********************************************************************************************************/
/*******************************二维卡尔曼范例************************************************************/
/*********************************************************************************************************/
/*********************************************************************************************************/
/*********************************************************************************************************/
//http://blog.csdn.net/xiahouzuoxin/article/details/39582483
typedef struct{
	float x;
	float p;
	float A;
	float H;
	float q;
	float r;
	float gain;
}kalman1_state;

typedef struct{
	float x[2];
	float p[2][2];
	float A[2][2];
	float H[2];
	float q[2];
	float r;
	float gain[2];
}kalman2_state;
/*********************************************************************************************************
 * @brief   
 *   Init fields of structure @kalman1_state.
 *   I make some defaults in this init function:
 *     A = 1;
 *     H = 1; 
 *   and @q,@r are valued after prior tests.
 *
 *   NOTES: Please change A,H,q,r according to your application.
 *
 * @inputs  
 *   state - Klaman filter structure
 *   init_x - initial x state value   
 *   init_p - initial estimated error convariance
 * @outputs 
 * @retval  
 */
void kalman1_init(kalman1_state *state, float init_x, float init_p)
{
    state->x = init_x;
    state->p = init_p;
    state->A = 1;
    state->H = 1;
    state->q = 2e2;//10e-6;  /* predict noise convariance */
    state->r = 5e2;//10e-5;  /* measure error convariance */
}
/*********************************************************************************************************
 * @brief   
 *   1 Dimension Kalman filter
 * @inputs  
 *   state - Klaman filter structure
 *   z_measure - Measure value
 * @outputs 
 * @retval  
 *   Estimated result
 */
float kalman1_filter(kalman1_state *state, float z_measure)
{
    /* Predict */
    state->x = state->A * state->x;
    state->p = state->A * state->A * state->p + state->q;  /* p(n|n-1)=A^2*p(n-1|n-1)+q */

    /* Measurement */
    state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
    state->x = state->x + state->gain * (z_measure - state->H * state->x);
    state->p = (1 - state->gain * state->H) * state->p;

    return state->x;
}

/*********************************************************************************************************
 * @brief   
 *   Init fields of structure @kalman1_state.
 *   I make some defaults in this init function:
 *     A = {{1, 0.1}, {0, 1}};
 *     H = {1,0}; 
 *   and @q,@r are valued after prior tests. 
 *
 *   NOTES: Please change A,H,q,r according to your application.
 *
 * @inputs  
 * @outputs 
 * @retval  
 */
void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2])
{
    state->x[0]    = init_x[0];
    state->x[1]    = init_x[1];
    state->p[0][0] = init_p[0][0];
    state->p[0][1] = init_p[0][1];
    state->p[1][0] = init_p[1][0];
    state->p[1][1] = init_p[1][1];
    //state->A       = {{1, 0.1}, {0, 1}};
    state->A[0][0] = 1;
    state->A[0][1] = 0.1;
    state->A[1][0] = 0;
    state->A[1][1] = 1;
    //state->H       = {1,0};
    state->H[0]    = 1;
    state->H[1]    = 0;
    //state->q       = {{10e-6,0}, {0,10e-6}};  /* measure noise convariance */
    state->q[0]    = 10e-7;
    state->q[1]    = 10e-7;
    state->r       = 10e-7;  /* estimated error convariance */
}

/*********************************************************************************************************
 * @brief   
 *   2 Dimension kalman filter
 * @inputs  
 *   state - Klaman filter structure
 *   z_measure - Measure value
 * @outputs 
 *   state->x[0] - Updated state value, Such as angle,velocity
 *   state->x[1] - Updated state value, Such as diffrence angle, acceleration
 *   state->p    - Updated estimated error convatiance matrix
 * @retval  
 *   Return value is equals to state->x[0], so maybe angle or velocity.
 */
float kalman2_filter(kalman2_state *state, float z_measure)
{
    float temp0;
    float temp1;
    float temp;

    /* Step1: Predict */
    state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];
    state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];
    /* p(n|n-1)=A^2*p(n-1|n-1)+q */
    state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];
    state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];
    state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];
    state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];

    /* Step2: Measurement */
    /* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */
    temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];
    temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];
    temp  = state->r + state->H[0] * temp0 + state->H[1] * temp1;
    state->gain[0] = temp0 / temp;
    state->gain[1] = temp1 / temp;
    /* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
    temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];
    state->x[0] = state->x[0] + state->gain[0] * (z_measure - temp); 
    state->x[1] = state->x[1] + state->gain[1] * (z_measure - temp);

    /* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */
    state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];
    state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];
    state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];
    state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];

    return state->x[0];
}
/*********************************************************************************************************/
/*********************************************************************************************************/
/*********************************************************************************************************/
/*******************************三维卡尔曼范例************************************************************/
/*********************************************************************************************************/
/*********************************************************************************************************/
/*********************************************************************************************************/
//https://www.embbnux.com/2015/01/30/9_dof_imu_with_kalman_filter_on_avr/
//算法实现
//kalman.c
float dtTimer   = 0.008;
float xk[9] = {0,0,0,0,0,0,0,0,0};
float pk[9] = {1,0,0,0,1,0,0,0,0};
float I[9]  = {1,0,0,0,1,0,0,0,1};
float R[9]  = {0.5,0,0,0,0.5,0,0,0,0.01};
float Q[9] = {0.005,0,0,0,0.005,0,0,0,0.001};
 
void matrix_add(float* mata,float* matb,float* matc){
    uint8_t i,j;
    for (i=0; i<3; i++){
       for (j=0; j<3; j++){
          matc[i*3+j] = mata[i*3+j] + matb[i*3+j];
       }
    }
}
 
void matrix_sub(float* mata,float* matb,float* matc){
    uint8_t i,j;
    for (i=0; i<3; i++){
       for (j=0; j<3; j++){
          matc[i*3+j] = mata[i*3+j] - matb[i*3+j];
       }
    }
}
 
void matrix_multi(float* mata,float* matb,float* matc){
  uint8_t i,j,m;
  for (i=0; i<3; i++)
  {
    for (j=0; j<3; j++)
   {
      matc[i*3+j]=0.0;
      for (m=0; m<3; m++)
     {
        matc[i*3+j] += mata[i*3+m] * matb[m*3+j];
      }
    }
 }
}
 
void KalmanFilter(float* am_angle_mat,float* gyro_angle_mat)
{
		uint8_t i,j;
		float yk[9];
		float pk_new[9];
		float K[9];
		float KxYk[9];
		float I_K[9];
		float S[9];
		float S_invert[9];
		float sdet;
		 
		//xk = xk + uk
		matrix_add(xk,gyro_angle_mat,xk);
		//pk = pk + Q
		matrix_add(pk,Q,pk);
		//yk =  xnew - xk
		matrix_sub(am_angle_mat,xk,yk);
		//S=Pk + R
		matrix_add(pk,R,S);
		//S??invert
		sdet = S[0] * S[4] * S[8]
							+ S[1] * S[5] * S[6]
							+ S[2] * S[3] * S[7]
							- S[2] * S[4] * S[6]
							- S[5] * S[7] * S[0]
							- S[8] * S[1] * S[3];
		 
		S_invert[0] = (S[4] * S[8] - S[5] * S[7])/sdet;
		S_invert[1] = (S[2] * S[7] - S[1] * S[8])/sdet;
		S_invert[2] = (S[1] * S[7] - S[4] * S[6])/sdet;
		 
		S_invert[3] = (S[5] * S[6] - S[3] * S[8])/sdet;
		S_invert[4] = (S[0] * S[8] - S[2] * S[6])/sdet;
		S_invert[5] = (S[2] * S[3] - S[0] * S[5])/sdet;
		 
		S_invert[6] = (S[3] * S[7] - S[4] * S[6])/sdet;
		S_invert[7] = (S[1] * S[6] - S[0] * S[7])/sdet;
		S_invert[8] = (S[0] * S[4] - S[1] * S[3])/sdet;
		//K = Pk * S_invert
		matrix_multi(pk,S_invert,K);
		//KxYk = K * Yk
		matrix_multi(K,yk,KxYk);
		//xk = xk + K * yk
		matrix_add(xk,KxYk,xk);
		//pk = (I - K)*(pk)
		matrix_sub(I,K,I_K);
		matrix_multi(I_K,pk,pk_new);
		//update pk
		//pk = pk_new;
		for (i=0; i<3; i++)
		{
				for (j=0; j<3; j++)
				{
						pk[i*3+j] = pk_new[i*3+j];
				}
		}
}

PID.c

#include "pid.h"
#include "myMath.h"	

/**************************************************************
 *批量复位PID函数
 * @param[in] 
 * @param[out] 
 * @return     
 ***************************************************************/	
void pidRest(PidObject **pid,const uint8_t len)
{
	uint8_t i;
	for(i=0;i<len;i++)
	{
	  	pid[i]->integ = 0;
	    pid[i]->prevError = 0;
	    pid[i]->out = 0;
		pid[i]->offset = 0;
	}
}

/**************************************************************
 * Update the PID parameters.
 *
 * @param[in] pid         A pointer to the pid object.
 * @param[in] measured    The measured value
 * @param[in] updateError Set to TRUE if error should be calculated.
 *                        Set to False if pidSetError() has been used.
 * @return PID algorithm output
 ***************************************************************/	
void pidUpdate(PidObject* pid,const float dt)
{
	 float error;
	 float deriv;
	
    error = pid->desired - pid->measured; //当前角度与实际角度的误差

    pid->integ += error * dt;	 //误差积分累加值
	
	//  pid->integ = LIMIT(pid->integ,pid->IntegLimitLow,pid->IntegLimitHigh); //进行积分限幅

    deriv = (error - pid->prevError)/dt;  //前后两次误差做微分
	
    pid->out = pid->kp * error + pid->ki * pid->integ + pid->kd * deriv;//PID输出
	
		//pid->out = LIMIT(pid->out,pid->OutLimitLow,pid->OutLimitHigh); //输出限幅
		
    pid->prevError = error;  //更新上次的误差
		
}

/**************************************************************
 *  CascadePID
 * @param[in] 
 * @param[out] 
 * @return     
 ***************************************************************/	
void CascadePID(PidObject* pidRate,PidObject* pidAngE,const float dt)  //串级PID
{	 
	pidUpdate(pidAngE,dt);    //先计算外环
	pidRate->desired = pidAngE->out;
	pidUpdate(pidRate,dt);    //再计算内环	
}

IMU.c

/******************************************************************************************************
 * Update the PID parameters.
 *
 * @param[in] pid         A pointer to the pid object.
 * @param[in] measured    The measured value
 * @param[in] updateError Set to TRUE if error should be calculated.
 *                        Set to False if pidSetError() has been used.
 * @return PID algorithm output
 *******************************************************************************************************/	
#include "imu.h"
#include "myMath.h"
#include <math.h>


static float NormAcc;

	
//float ex_int = 0, ey_int = 0, ez_int = 0;   //X、Y、Z轴的比例误差
//float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    //定义四元素
//float his_q0 = 1, his_q1 = 0, his_q2 = 0, his_q3 = 0; 
//float q0_yaw = 1, q1_yaw = 0, q2_yaw = 0, q3_yaw = 0;    //弥补Mahony算法在无地磁情况解算Yaw轴满足不了大扰动要求的现象
//float his_q0_yaw = 1, his_q1_yaw = 0, his_q2_yaw = 0, his_q3_yaw = 0;

//void GetAngle(const stMpu *pMpu,void *pAngE, float dt) 
//{
//	static const float KpDef = 0.85f ;
//	static const float KiDef = 0.0035f;
//	const float Gyro_Gr = 0.0005326f;     //面计算度每秒,转换弧度每秒则 0.03051756	 * 0.0174533f = 0.0005326
//	float HalfTime = 0.5 * dt;
//	float gx = pMpu->gyroX	*	Gyro_Gr;
//	float gy = pMpu->gyroY	*	Gyro_Gr;
//	float gz = pMpu->gyroZ	*	Gyro_Gr;
//	float ax =  pMpu->accX;
//	float	ay =  pMpu->accY;
//	float az =  pMpu->accZ;
//	
//	float vx, vy, vz;
//  float ex, ey, ez;
//		
//	static float his_q0q0;
//  static float his_q0q1;
//  static float his_q0q2;
//  static float his_q1q1;
//  static float his_q1q3;
//  static float his_q2q2;
//  static float his_q2q3;
//  static float his_q3q3;
//	
//	float q0q0;
//  float q0q1;
//  float q0q2;
//  float q1q1;
//  float q1q3;
//  float q2q2;
//  float q2q3;
//  float q3q3;		
//	
//	float	q0_yawq0_yaw;
//	float	q1_yawq1_yaw;
//	float	q2_yawq2_yaw;
//	float	q3_yawq3_yaw;
//	float	q1_yawq2_yaw;
//	float	q0_yawq3_yaw;
//	
////**************************Yaw轴计算******************************
//	
//	//Yaw轴四元素的微分方程
//  q0_yaw = his_q0_yaw + (-his_q1_yaw * gx - his_q2_yaw * gy - his_q3_yaw * gz) * HalfTime;
//  q1_yaw = his_q1_yaw + ( his_q0_yaw * gx + his_q2_yaw * gz - his_q3_yaw * gy) * HalfTime;
//  q2_yaw = his_q2_yaw + ( his_q0_yaw * gy - his_q1_yaw * gz + his_q3_yaw * gx) * HalfTime;
//  q3_yaw = his_q3_yaw + ( his_q0_yaw * gz + his_q1_yaw * gy - his_q2_yaw * gx) * HalfTime;
//	
//	q0_yawq0_yaw = q0_yaw * q0_yaw;
//	q1_yawq1_yaw = q1_yaw * q1_yaw;
//	q2_yawq2_yaw = q2_yaw * q2_yaw;
//	q3_yawq3_yaw = q3_yaw * q3_yaw;
//	q1_yawq2_yaw = q1_yaw * q2_yaw;
//	q0_yawq3_yaw = q0_yaw * q3_yaw;
//	
//	//规范化Yaw轴四元数
//  norm = Q_rsqrt(q0_yawq0_yaw + q1_yawq1_yaw + q2_yawq2_yaw + q3_yawq3_yaw);
//  q0_yaw = q0_yaw * norm;
//  q1_yaw = q1_yaw * norm;
//  q2_yaw = q2_yaw * norm;
//  q3_yaw = q3_yaw * norm;	
//  
//	//if(ax * ay * az	== 0)
//	//return ;
//	
//	//归一化加速度计值
//  norm = Q_rsqrt(ax * ax + ay * ay + az * az); 
//  ax = ax * norm;
//  ay = ay * norm;
//  az = az * norm;
//	
//	//由姿态阵估计重力方向和流量/变迁
//  vx = 2 * (his_q1q3 - his_q0q2);											
//  vy = 2 * (his_q0q1 + his_q2q3);
//  vz = his_q0q0 - his_q1q1 - his_q2q2 + his_q3q3;
//	
//  //向量外积再相减得到差分就是误差 两个单位向量的差积即为误差向量
//  ex = (ay * vz - az * vy) ;      
//  ey = (az * vx - ax * vz) ;
//  ez = (ax * vy - ay * vx) ;

//	//对误差进行PI计算
//  ex_int = ex_int + ex * KiDef;			
//  ey_int = ey_int + ey * KiDef;
//  ez_int = ez_int + ez * KiDef;

//  //校正陀螺仪
//  gx = gx + KpDef * ex + ex_int;					
//  gy = gy + KpDef * ey + ey_int;
//  gz = gz + KpDef * ez + ez_int;			
//			
//	//四元素的微分方程
//  q0 = his_q0 + (-his_q1 * gx - his_q2 * gy - his_q3 *	gz)	*	HalfTime;
//  q1 = his_q1 + ( his_q0 * gx + his_q2 * gz - his_q3 *	gy)	*	HalfTime;
//  q2 = his_q2 + ( his_q0 * gy - his_q1 * gz + his_q3 *	gx)	*	HalfTime;
//  q3 = his_q3 + ( his_q0 * gz + his_q1 * gy - his_q2 *	gx)	*	HalfTime;

//	q0q0 = q0 * q0;
//	q0q1 = q0 * q1;
//	q0q2 = q0 * q2;
//	q1q1 = q1 * q1;
//	q1q3 = q1 * q3;
//	q2q2 = q2	* q2;
//	q2q3 = q2	*	q3;
//	q3q3 = q3	*	q3;	

//  //规范化Pitch、Roll轴四元数
//  norm = Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3);
//  q0 = q0 * norm;
//  q1 = q1 * norm;
//  q2 = q2 * norm;
//  q3 = q3 * norm;
//	
//	//求解欧拉角
//	*((float *)pAngE+2) = atan2f(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * RtA;  //ROLL
//	*((float *)pAngE+1) = asin(2 * q0q2 -2 * q1q3) * 57.3;  //PITCH
//	*(float *)pAngE = atan2f(2 * q1_yawq2_yaw + 2 * q0_yawq3_yaw, -2 * q2_yawq2_yaw - 2 * q3_yawq3_yaw + 1)	* RtA;  //YAW
//	
//	
//  //存储更替相应的四元数
//	his_q0_yaw = q0_yaw;
//  his_q1_yaw = q1_yaw;
//  his_q2_yaw = q2_yaw;
//  his_q3_yaw = q3_yaw;

//	his_q0 = q0;
//  his_q1 = q1;
//  his_q2 = q2;
//  his_q3 = q3;
//	
//	his_q0q0 = q0q0;
//	his_q0q1 = q0q1;
//	his_q0q2 = q0q2;
//	his_q1q1 = q1q1;
//	his_q1q3 = q1q3;
//	his_q2q2 = q2q2;
//	his_q2q3 = q2q3;
//	his_q3q3 = q3q3;	
//}








typedef volatile struct {
  float q0;
  float q1;
  float q2;
  float q3;
} Quaternion;


void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt) 
{		
	volatile struct V{
				float x;
				float y;
				float z;
				} Gravity,Acc,Gyro,AccGravity;

	static struct V GyroIntegError = {0};
	static  float KpDef = 0.8f;
	static  float KiDef = 0.0003f;
	static Quaternion NumQ = {1, 0, 0, 0};
	float q0_t,q1_t,q2_t,q3_t;
  //float NormAcc;	
	float NormQuat; 
	float HalfTime = dt * 0.5f;

	

	// 提取等效旋转矩阵中的重力分量 
	Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);								
	Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);						  
	Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);	
	// 加速度归一化
 NormAcc = Q_rsqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
	
    Acc.x = pMpu->accX * NormAcc;
    Acc.y = pMpu->accY * NormAcc;
    Acc.z = pMpu->accZ * NormAcc;	
 	//向量差乘得出的值
	AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
	AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
	AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
	//再做加速度积分补偿角速度的补偿值
    GyroIntegError.x += AccGravity.x * KiDef;
    GyroIntegError.y += AccGravity.y * KiDef;
    GyroIntegError.z += AccGravity.z * KiDef;
	//角速度融合加速度积分补偿值
    Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x  +  GyroIntegError.x;//弧度制
    Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y  +  GyroIntegError.y;
    Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z  +  GyroIntegError.z;		
	// 一阶龙格库塔法, 更新四元数

	q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
	q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
	q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
	q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;
	
	NumQ.q0 += q0_t;
	NumQ.q1 += q1_t;
	NumQ.q2 += q2_t;
	NumQ.q3 += q3_t;
	// 四元数归一化
	NormQuat = Q_rsqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
	NumQ.q0 *= NormQuat;
	NumQ.q1 *= NormQuat;
	NumQ.q2 *= NormQuat;
	NumQ.q3 *= NormQuat;	
	

		// 四元数转欧拉角
	{
		 
			#ifdef	YAW_GYRO
			*(float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA;  //yaw
			#else
				float yaw_G = pMpu->gyroZ * Gyro_G;
				if((yaw_G > 3.0f) || (yaw_G < -3.0f)) //数据太小可以认为是干扰，不是偏航动作
				{
					pAngE->yaw  += yaw_G * dt;			
				}
			#endif
			pAngE->pitch  =  asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;						
		
			pAngE->roll	= atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA;	//PITCH 			
	}
}

Mymath.c

#include "myMath.h"
#include <math.h>
#include "delay.h"

const float M_PI = 3.1415926535;
const float RtA = 57.2957795f;
const float AtR = 0.0174532925f;
const float Gyro_G = 0.03051756f*2;	  	//陀螺仪初始化量程+-2000度每秒于1 / (65536 / 4000) = 0.03051756*2		
const float Gyro_Gr = 0.0005326f*2;     //面计算度每秒,转换弧度每秒则 2*0.03051756	 * 0.0174533f = 0.0005326*2

/*====================================================================================================*/
/*====================================================================================================*
**函数 : Q_rsqrt
**功能 : 快速计算 三角函数
**输入 : number  
**输出 : 结果
**备注 : None
**====================================================================================================*/
/*====================================================================================================*/
//逼近法；线性拟合
//Q (4/M_PI x - 4/M_PI^2 x^2) + P (4/M_PI x - 4/M_PI^2 x^2)^2 
#ifndef TAPYOR
float sine(float x)          // (-M_PI , M_PI) ???? 0.0005
{
	const float Q = 0.775;
	const float P = 0.225;
	const float B =  4 / M_PI;  
	const float C = -4 /(M_PI*M_PI);
	float y = B * x + C * x * fabs(x); 
	return (Q * y + P * y * fabs(y));
}
#else 
//4级泰勒公式法 在PI出会出现0.7的最大误差
//sinx= x- x^3/3! + x^5/5! - x^7/7!+ x^9/9! . =?(-1)^n x^(2n+1)/(2n+1)!
float sine(float x)
{
	float t=x;
	float result = x;
	float X2 = x*x;
	uint8_t cnt = 1;

	do
	{
		t=-t;
		t *= X2;
		result += t/((cnt<<1)+1);
		cnt++;
	}	while(cnt<5);//6阶

	return result;
} 
#endif
//http://wenku.baidu.com/link?url=jUswZ3G2z26IUS72IkeZrizc5V9VdR1sTF8xGCOHPFW0P70bGjjm5zhNxvRT36X31TMoFf6S-9lMoIkK4pPwExAaEZGtRpWggdQAzpg3Fsu
//cos(x)=sin(M_PI/2+x)=sin(M_PI/2-x)
//cos(x-M_PI/2)=sin(x)
float cosine(float x)
{
	return sine(x+M_PI/2);//奇变偶不变，符号看象限
}

//反正切麦克劳林展开式 阶数越高，值越准确   70°以内是准确的
//http://www.zybang.com/question/246f9997776f7d5cc636b10aff27a1cb.html
float arctan(float x)  //  (-1 , +1)    6? ?? 0.002958 
{
	float t = x;
	float result = 0;
	float X2 = x * x;
	unsigned char cnt = 1;
	do
	{
		result += t / ((cnt << 1) - 1);
		t = -t;
		t *= X2;
		cnt++;
	}while(cnt <= 6);//5??
	return result;
}

//反正弦麦克劳林展开式 -1<x<+1     42°以内是准确的
//http://xuxzmail.blog.163.com/blog/static/25131916200971794014536/
const float PI_2 = 1.570796f;
float arcsin(float x)   //(-1 , +1)  ? 0 ????  6? ??0.005
{
	float d=1;
	float t=x;
	unsigned char cnt = 1;
	float result = 0;	
	float X2 = x*x;
	
	if (x >= 1.0f) 
	{
		return PI_2;
	}
	if (x <= -1.0f) 
	{
		return -PI_2;
	}
	do
	{
		result += t / (d * ((cnt << 1) - 1));
		t *= X2 * ((cnt << 1) - 1);//
		d *= (cnt << 1);//2 4 6 8 10 ...
		cnt++;
	}while(cnt <= 6);

	return result;
}

//保证输入值是有效的
float safe_asin(float v)
{
    if (isnan(v)) {
        return 0.0;
    }
    if (v >= 1.0f) {
        return M_PI/2;
    }
    if (v <= -1.0f) {
        return -M_PI/2;
    }
    return asinf(v);
}


/*====================================================================================================*/
/*====================================================================================================*
**函数 : Q_rsqrt
**功能 : 快速计算 1/Sqrt(x) 
**输入 : number  
**输出 : 结果
**备注 : None
**====================================================================================================*/
/*====================================================================================================*/
float Q_rsqrt(float number)
{
	long i;
	float x2, y;
	const float threehalfs = 1.5F;
 
	x2 = number * 0.5F;
	y  = number;
	i  = * ( long * ) &y;                      
	i  = 0x5f3759df - ( i >> 1 );               
	y  = * ( float * ) &i;
	y  = y * ( threehalfs - ( x2 * y * y ) );   // 1st iteration （第一次牛顿迭代）
	return y;
} 

/**************************实现函数********************************************
*函数原型:    array_astrict_lower(int16_t *array,int16_t value)
*功　　能:    对数组下限限制
输入参数：    *array   目标数组指针
*             value      
输出参数：    无
*******************************************************************************/
void array_astrict(int16_t *array,int16_t lower,int16_t upper)
{
   int16_t length = sizeof(array); 
	 uint16_t i = 0;
   for(i=0;i<length;i++)
   {
     if(*(array+i)<lower)  *(array+i) = lower;
     else if(*(array+i)>upper)  *(array+i) = upper;
   } 
}

/**************************实现函数********************************************
*函数原型:    array_assign(int16_t *array,int16_t value)
*功　　能:    对数组赋值
输入参数：    *array   目标数组指针 
*             value      
输出参数：    无
*******************************************************************************/
void array_assign(int16_t *array,int16_t value)
{
   uint16_t length = sizeof(array); 
	 uint16_t i=0;
   for(i=0;i<length;i++)
   {
     *(array+i) = value;
   } 
}

/**************************实现函数********************************************
*函数原型:    data_limit(float data,flaot toplimit,float lowerlimit)
*功　　能:    数据限幅
输入参数：    data       要操作的数据 
*             toplimit   上限
*             lowerlimit 下限
输出参数：    无
*******************************************************************************/
float data_limit(float data,float toplimit,float lowerlimit)
{
  if(data > toplimit)  data = toplimit;
  else if(data < lowerlimit) data = lowerlimit;
	return data;
}


/***********************************************
  * @brief  可变增益自适应参数
  * @param  None
  * @retval None
************************************************/
float VariableParameter(float error)
{
	float  result = 0;
	
	if(error < 0)
	{
	   error = -error;
	}
  if(error >0.6f)
	{
	   error = 0.6f;
	}
	result = 1 - 1.667f * error;
	if(result < 0)
	{
	   result = 0;
	}
	return result;
}


float middle_3(float input) //3个数取中间的数
{ 

  int a,b,c,t; 


  if(a<b)

  { 

     t=a;a=b;b=t; 

  } 

 if(b<c)//9 8 7 

 { 

  t=b;b=c;c=t;      

 } 

 if(a<b)//9 8 7 

 { 

  t=a;a=b;b=t; 

 } 

 return b; 

}



float my_deathzoom_2(float x,float zoom)
{
	float t;
	
	if( x> -zoom && x < zoom )
	{
		t = 0;
	}
	else
	{
		t = x;
	}
  return (t);
}


float my_deathzoom(float x,float zoom)
{
	float t;
	if(x>0)
	{
		t = x - zoom;
		if(t<0)
		{
			t = 0;
		}
	}
	else
	{
		t = x + zoom;
		if(t>0)
		{
			t = 0;
		}
	}
  return (t);
}

遥控器部分

硬件设计

电源设计

STM32主控设计

摇杆电路设计

2.4G、OLED等其他模块设计

BOM表

Comment	Description	Designator	Footprint	Quantity	Value
CAP	0603电容	C1, C3, C4, C5, C8, C9, C10, C11, C12	C_0603	9	100nF
CAP	0603电容	C2, C14, C16	C_0603	3	100nF
CAP	0603电容	C6, C7	C_0603	2	22pF
CAP	1206钽电容	C13, C15	C_1206	2	100pF
S4	二极管	D1	SOD-123	1
DC4.2V	2p排针	JP1	SIP2	1
SW-PB	侧按按键开关	KEY1, KEY2	KEY(364.3mm)侧脚开关	2
MSS22D18	6p贴片拨动开关	KG1	MSS22D18	1
蓝灯	发光二极管	LED1, LED3	0603_LED_S1	2
红灯	发光二极管	LED2	0603_LED_S1	1
NRF24L01	2.4G模块	NRF1	24L01	1
OLED	OLED模块	P1	SIP7	1
Res2	0603电阻	R1	R0603	1	10K
Res2	0603电阻	R2, R3, R4	R0603	3	1K
Res2	0603电阻	R5, R6, R7, R8, R9, R10	R0603	6	10K
SW1	按键开关	S1, S2, S3, S4	KEY_4(SOP)	4
Header 4	4p排针	SWD1, USART1	HDR1X4	2
STM32F103C8T6_LQFP48	STM32C8T6	U1	LQFP48_STM32	1
662K	LDO	U2	SOT-23-3L	1
XTAL	贴片晶振CSTCE8M00G55	Y1	CSTCE8M00G55	1	8M
摇杆	摇杆	YG1, YG2	GY	2

软件设计

main.c

#include "stm32f10x.h"
#include "systick.h"
#include "stdio.h"
#include "delay.h"
#include "usart.h"
#include "nrf24l01.h"
#include "ADC_DMA_Config.h"
#include "remote.h"
#include "Beep.h"
#include "oled.h"
#include "bmp.h"
int main(void)
{	
	cycleCounterInit();
	while(SysTick_Config(SystemCoreClock / 1000))		//系统滴答时钟 1ms发生一次中断
	{
		
	}
	BEEP_INIT();
	NRF24L01_INIT();        //初始化NRF24L01
  USART1_Config();        //初始化串口1	
	OLED_Config();					//OLED配置
	delay_ms(100);	
	printf("\r\nstm32f103C8t6\r\n");//串口1打印单片机型号
	ADC1_GPIO_Config();      //初始化ADC IO	
	ADC1_Mode_Config();      //初始化ADC模式	
	RC_INIT();               //校准摇杆数据初始化
	while(1)
	{
		OLED_ShowData();
	}
}

ADC_DMA.c

/*

THR 	- PA0	-	ADC12_IN0
YAW		-	PA1	-	ADC12_IN1
PITCH	-	PA2	-	ADC12_IN2
ROLL	-	PA3	-	ADC12_IN3
POWER	-	PA4	-	ADC12_IN4
KEY_L	-	PB0	-	ADC12_IN8
KEY_R	-	PB1	-	ADC12_IN9

*/
#include "ADC_DMA_Config.h"

#define ADC1_DR_Address    ((u32)0x40012400+0x4c)

__IO uint16_t ADC_ConvertedValue[4];


/*
 * 函数名：ADC1_GPIO_Config
 * 描述  ：使能ADC1和DMA1的时钟，初始化PC.01
 * 输入  : 无
 * 输出  ：无
 * 调用  ：内部调用
 */
void ADC1_GPIO_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	/* Enable DMA clock */
	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
	
	/* Enable ADC1 and GPIOC clock */
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_GPIOA, ENABLE);
	
	/* Configure PC.01  as analog input */

	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;		
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3; //THR,YAW,ROLL,PITCH
	GPIO_Init(GPIOA, &GPIO_InitStructure);			
}


/* 函数名：ADC1_Mode_Config
 * 描述  ：配置ADC1的工作模式为MDA模式
 * 输入  : 无
 * 输出  ：无
 * 调用  ：内部调用
 */
void ADC1_Mode_Config(void)
{
	DMA_InitTypeDef DMA_InitStructure;
	ADC_InitTypeDef ADC_InitStructure;
	
	/* DMA channel1 configuration */
	DMA_DeInit(DMA1_Channel1);
	DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address;	 //ADC地址
	DMA_InitStructure.DMA_MemoryBaseAddr = (u32)&ADC_ConvertedValue;//内存地址
	DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
	DMA_InitStructure.DMA_BufferSize = 4;
	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;//外设地址固定
	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;  //内存地址固定
	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;	//半字
	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
	DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;		//循环传输
	DMA_InitStructure.DMA_Priority = DMA_Priority_High;
	DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
	DMA_Init(DMA1_Channel1, &DMA_InitStructure);
	
	/* Enable DMA channel1 */
	DMA_Cmd(DMA1_Channel1, ENABLE);
	
	/* ADC1 configuration */
	
	ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;	//独立ADC模式
	ADC_InitStructure.ADC_ScanConvMode = ENABLE ; 	 //禁止扫描模式，扫描模式用于多通道采集
	ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;	//开启连续转换模式，即不停地进行ADC转换
	ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;	//不使用外部触发转换
	ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; 	//采集数据右对齐
	ADC_InitStructure.ADC_NbrOfChannel = 4;	 	//要转换的通道数目
	ADC_Init(ADC1, &ADC_InitStructure);
	
	/*配置ADC时钟，为PCLK2的6分频，即12MHz,ADC频率最高不能超过14MHz*/
	RCC_ADCCLKConfig(RCC_PCLK2_Div8); 
	/*配置ADC1的通道11为55.	5个采样周期，序列为1 */ 

	ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_55Cycles5);
	ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 2, ADC_SampleTime_55Cycles5);
	ADC_RegularChannelConfig(ADC1, ADC_Channel_2, 3, ADC_SampleTime_55Cycles5);
	ADC_RegularChannelConfig(ADC1, ADC_Channel_3, 4, ADC_SampleTime_55Cycles5);
//	ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 5, ADC_SampleTime_55Cycles5);
//	ADC_RegularChannelConfig(ADC1, ADC_Channel_8, 6, ADC_SampleTime_55Cycles5);
//	ADC_RegularChannelConfig(ADC1, ADC_Channel_9, 7, ADC_SampleTime_55Cycles5);



	/* Enable ADC1 DMA */
	ADC_DMACmd(ADC1, ENABLE);
	
	/* Enable ADC1 */
	ADC_Cmd(ADC1, ENABLE);
	
	/*复位校准寄存器 */   
	ADC_ResetCalibration(ADC1);
	/*等待校准寄存器复位完成 */
	while(ADC_GetResetCalibrationStatus(ADC1));
	
	/* ADC校准 */
	ADC_StartCalibration(ADC1);
	/* 等待校准完成*/
	while(ADC_GetCalibrationStatus(ADC1));
	
	/* 由于没有采用外部触发，所以使用软件触发ADC转换 */ 
	ADC_SoftwareStartConvCmd(ADC1, ENABLE);
}

EEPROM.c

#include "EEPROM.h"
#include "stm32f10x_flash.h"

typedef enum {FAILED = 0, PASSED = !FAILED} Status;//定义要使用的状态枚举变量




uint32_t EraseCounter = 0x00, Address = 0x00;
uint32_t Data = 0x3210ABCD;
uint32_t First_Page = 0x00;                       //起始页


__IO FLASH_Status FLASHStatus = FLASH_COMPLETE;
__IO Status MemoryProgramStatus = PASSED;
//函数声明


//=============================================================================
//文件名称：main
//功能概要：主函数
//参数说明：无
//函数返回：int
//=============================================================================
void FLASH_write(int16_t *data,uint8_t len)
{
  FLASH_Unlock();//先解锁
  FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); //清楚相应的标志位
  First_Page = (FLASH_End_Addr - FLASH_Start_Addr+1) / FLASH_Page_Size;//计算出起始页
  /* 使用前先擦除 */
  for(EraseCounter = 0; (EraseCounter < First_Page) && (FLASHStatus == FLASH_COMPLETE); EraseCounter++)
  {
    if (FLASH_ErasePage(FLASH_Start_Addr + (FLASH_Page_Size * EraseCounter))!= FLASH_COMPLETE)
    {
      while (1)
      {			
      }
    }
  }
	
  /* 写入FLASH */
  Address = FLASH_Start_Addr;
  while (len--)
  {
    if (FLASH_ProgramHalfWord(Address, *data) == FLASH_COMPLETE)
    {
		data++;
		Address = Address + 2;
    }
    else
    { 
      while (1)
      {
      }
    }
  }
  FLASH_Lock();
	/* 上锁 */
}	
void FLASH_read(uint32_t *data,uint8_t len)
{
  Address = FLASH_Start_Addr;
  
  while (len--)
  {
    *data = *(__IO uint32_t *)Address;
	data++;
    Address = Address + 4;
  }
}

NRF24L01.c

#include "nrf24l01.h"
#include "SPI.h"
#include <string.h>
	
#undef SUCCESS
#define SUCCESS 0
#undef FAILED
#define FAILED  1
//配对密码
const uint8_t TX_ADDRESS[]= {0x11,0x22,0x33,0x44,0x55};	//本地地址
const uint8_t RX_ADDRESS[]= {0x11,0x22,0x33,0x44,0x55};	//接收地址RX_ADDR_P0 == RX_ADDR

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//24L01操作线   
#define Set_NRF24L01_CSN    (GPIOA->BSRR = GPIO_Pin_4) // PB12
#define Clr_NRF24L01_CSN     (GPIOA->BRR = GPIO_Pin_4)    // PB12
#define Set_NRF24L01_CE (GPIOA->BSRR = GPIO_Pin_15)    // PB1
#define Clr_NRF24L01_CE  (GPIOA->BRR = GPIO_Pin_15)    // PB1
#define READ_NRF24L01_IRQ   (GPIOA->IDR&GPIO_Pin_11)//IRQ主机数据输入 PB0

//#define GPIO_Remap_SWJ_JTAGDisable  ((uint32_t)0x00300200)  /*!< JTAG-DP Disabled and SW-DP Enabled */



//初始化24L01的IO口
void NRF24L01_Configuration(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);      /*??SWD ??JTAG*/
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);    //led
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9|GPIO_Pin_10;          //
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;    //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	


	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);    //使能GPIO的时钟  CE
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;          //NRF24L01 模块片选信号
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;    //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);   //使能GPIO的时钟 CSN
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;      
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;    //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	
	Set_NRF24L01_CE;                                    //初始化时先拉高
	Set_NRF24L01_CSN;                                   //初始化时先拉高

    //配置NRF2401的IRQ
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU  ;     //上拉输入
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	GPIO_SetBits(GPIOA,GPIO_Pin_11);


	
	SPI_Config();                                //初始化SPI
	Clr_NRF24L01_CE; 	                                //使能24L01
	Set_NRF24L01_CSN;                                   //SPI片选取消
}
//上电检测NRF24L01是否在位
//写5个数据然后再读回来进行比较，
//相同时返回值:0，表示在位;否则返回1，表示不在位	
u8 NRF24L01_Check(void)
{
	u8 buf[5]={0XA5,0XA5,0XA5,0XA5,0XA5};
	u8 buf1[5];
	u8 i;   	 
	NRF24L01_Write_Buf(SPI_WRITE_REG+TX_ADDR,buf,5);//写入5个字节的地址.	
	NRF24L01_Read_Buf(TX_ADDR,buf1,5);              //读出写入的地址  	
	for(i=0;i<5;i++)if(buf1[i]!=0XA5)break;					   
	if(i!=5)return 1;                               //NRF24L01不在位	
	return 0;		                                //NRF24L01在位
}	 	 
//通过SPI写寄存器
u8 NRF24L01_Write_Reg(u8 regaddr,u8 data)
{
	u8 status;	
    Clr_NRF24L01_CSN;                    //使能SPI传输
  	status =SPI_RW(regaddr); //发送寄存器号 
  	SPI_RW(data);            //写入寄存器的值
  	Set_NRF24L01_CSN;                    //禁止SPI传输	   
  	return(status);       		         //返回状态值
}
//读取SPI寄存器值 ，regaddr:要读的寄存器
u8 NRF24L01_Read_Reg(u8 regaddr)
{
	u8 reg_val;	    
	Clr_NRF24L01_CSN;                //使能SPI传输		
  	SPI_RW(regaddr);     //发送寄存器号
  	reg_val=SPI_RW(0XFF);//读取寄存器内容
  	Set_NRF24L01_CSN;                //禁止SPI传输		    
  	return(reg_val);                 //返回状态值
}	
//在指定位置读出指定长度的数据
//*pBuf:数据指针
//返回值,此次读到的状态寄存器值 
u8 NRF24L01_Read_Buf(u8 regaddr,u8 *pBuf,u8 datalen)
{
	u8 status,u8_ctr;	       
  	Clr_NRF24L01_CSN;                     //使能SPI传输
  	status=SPI_RW(regaddr);   //发送寄存器值(位置),并读取状态值   	   
	for(u8_ctr=0;u8_ctr<datalen;u8_ctr++)pBuf[u8_ctr]=SPI_RW(0XFF);//读出数据
  	Set_NRF24L01_CSN;                     //关闭SPI传输
  	return status;                        //返回读到的状态值
}
//在指定位置写指定长度的数据
//*pBuf:数据指针
//返回值,此次读到的状态寄存器值
u8 NRF24L01_Write_Buf(u8 regaddr, u8 *pBuf, u8 datalen)
{
	u8 status,u8_ctr;	    
	Clr_NRF24L01_CSN;                                    //使能SPI传输
  	status = SPI_RW(regaddr);                //发送寄存器值(位置),并读取状态值
  	for(u8_ctr=0; u8_ctr<datalen; u8_ctr++)SPI_RW(*pBuf++); //写入数据	 
  	Set_NRF24L01_CSN;                                    //关闭SPI传输
  	return status;                                       //返回读到的状态值
}				   
//启动NRF24L01发送一次数据
//txbuf:待发送数据首地址
//返回值:发送完成状况
u8 NRF24L01_TxPacket(u8 *txbuf)
{
	u8 state;   
	Clr_NRF24L01_CE;
	NRF24L01_Write_Buf(WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);//写数据到TX BUF  32个字节
	Set_NRF24L01_CE;                                     //启动发送	   
//	while(READ_NRF24L01_IRQ!=0);                         //等待发送完成
	state=NRF24L01_Read_Reg(STATUS);                     //读取状态寄存器的值	   
	NRF24L01_Write_Reg(SPI_WRITE_REG+STATUS,state);      //清除TX_DS或MAX_RT中断标志
	if(state&MAX_TX)                                     //达到最大重发次数
	{
		NRF24L01_Write_Reg(FLUSH_TX,0xff);               //清除TX FIFO寄存器 
		return MAX_TX; 
	}
	if(state&TX_OK)                                      //发送完成
	{
		return TX_OK;
	}
	return 0xff;                                         //其他原因发送失败
}

//启动NRF24L01发送一次数据
//txbuf:待发送数据首地址
//返回值:0，接收完成；其他，错误代码
u8 NRF24L01_RxPacket(u8 *rxbuf)
{
	u8 state;		    							      
	state=NRF24L01_Read_Reg(STATUS);                //读取状态寄存器的值    	 
	NRF24L01_Write_Reg(SPI_WRITE_REG+STATUS,state); //清除TX_DS或MAX_RT中断标志
	if(state&RX_OK)                                 //接收到数据
	{
		NRF24L01_Read_Buf(RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);//读取数据
		NRF24L01_Write_Reg(FLUSH_RX,0xff);          //清除RX FIFO寄存器 
		return 0; 
	}	   
	return 1;                                      //没收到任何数据
}

//该函数初始化NRF24L01到RX模式
//设置RX地址,写RX数据宽度,选择RF频道,波特率和LNA HCURR
//当CE变高后,即进入RX模式,并可以接收数据了		   
void RX_Mode(void)
{
	Clr_NRF24L01_CE;	  
    //写RX节点地址
  	NRF24L01_Write_Buf(SPI_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH);

    //使能通道0的自动应答    
  	NRF24L01_Write_Reg(SPI_WRITE_REG+EN_AA,0x01);    
    //使能通道0的接收地址  	 
  	NRF24L01_Write_Reg(SPI_WRITE_REG+EN_RXADDR,0x01);
    //设置RF通信频率		  
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RF_CH,45);	   //(要改单机控制，就把45改成跟遥控器单独一样的。就可以单机控制了)
    //选择通道0的有效数据宽度 	    
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);
    //设置TX发射参数,0db增益,2Mbps,低噪声增益开启   
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RF_SETUP,0x0f);
    //配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,PRIM_RX接收模式 
  	NRF24L01_Write_Reg(SPI_WRITE_REG+NCONFIG, 0x0f); 
    //CE为高,进入接收模式 
  	Set_NRF24L01_CE;                                
}			

//该函数初始化NRF24L01到TX模式
//设置TX地址,写TX数据宽度,设置RX自动应答的地址,填充TX发送数据,
//选择RF频道,波特率和LNA HCURR PWR_UP,CRC使能
//当CE变高后,即进入RX模式,并可以接收数据了		   
//CE为高大于10us,则启动发送.	 
void TX_Mode(void)
{														 
	Clr_NRF24L01_CE;	    
    //写TX节点地址 
  	NRF24L01_Write_Buf(SPI_WRITE_REG+TX_ADDR,(u8*)TX_ADDRESS,TX_ADR_WIDTH);    
    //设置TX节点地址,主要为了使能ACK	  
  	NRF24L01_Write_Buf(SPI_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH); 

    //使能通道0的自动应答    
  	NRF24L01_Write_Reg(SPI_WRITE_REG+EN_AA,0x01);     
    //使能通道0的接收地址  
  	NRF24L01_Write_Reg(SPI_WRITE_REG+EN_RXADDR,0x01); 
    //设置自动重发间隔时间:500us + 86us;最大自动重发次数:10次
  	NRF24L01_Write_Reg(SPI_WRITE_REG+SETUP_RETR,0x1a);
    //设置RF通道为40
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RF_CH,45);     //(要改单机控制，就把45改成跟遥控器单独一样的。就可以单机控制了)
    //设置TX发射参数,0db增益,2Mbps,低噪声增益开启   
  	NRF24L01_Write_Reg(SPI_WRITE_REG+RF_SETUP,0x0f);  //0x27  250K   0x07 1M
    //配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,PRIM_RX发送模式,开启所有中断
  	NRF24L01_Write_Reg(SPI_WRITE_REG+NCONFIG,0x0e);    
    // CE为高,10us后启动发送
	Set_NRF24L01_CE;                                  
}		  

void NRF24L01_INIT(void)
{
	NRF24L01_Configuration();
	
	while(NRF24L01_Check())
	{
		
	}
	GPIO_SetBits(GPIOB,GPIO_Pin_9);
  TX_Mode();
}

SPI.c

#include "spi.h"

void SPI_Config(void)
{
      SPI_InitTypeDef  SPI_InitStructure;
   	  GPIO_InitTypeDef GPIO_InitStructure;

		RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1,ENABLE);  
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO  , ENABLE);
		GPIO_SetBits(GPIOA, GPIO_Pin_4); //NRF_CS预置为高 
		
	  	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7; 
	  	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	  	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 
	  	GPIO_Init(GPIOA, &GPIO_InitStructure); 

        /* SPI2 configuration */
        SPI_Cmd(SPI1, DISABLE);             //必须先禁能,才能改变MODE
        SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
        SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
        SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
        SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
        SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
        SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
        SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
        SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
        SPI_InitStructure.SPI_CRCPolynomial = 7;

        SPI_Init(SPI1, &SPI_InitStructure);

        /* SPI2 enable */
        SPI_Cmd(SPI1, ENABLE);
}
u8 SPI_RW(u8 dat)
{
        /* Loop while DR register in not emplty */
        while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET);

        /* Send byte through the SPI2 peripheral */
        SPI_I2S_SendData(SPI1, dat);

        /* Wait to receive a byte */
        while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE) == RESET);

        /* Return the byte read from the SPI bus */
        return SPI_I2S_ReceiveData(SPI1);  
}

Systick.c

#include "systick.h"
#include "ADC_DMA_Config.h"
#include "SPI.h"
#include "nrf24l01.h"
#include "eeprom.h"
#include "remote.h"
#include <string.h>
#include "usart.h"
#include "delay.h"

uint32_t SysTick_count = 0;

void SysTick_Handler(void)
{
	SysTick_count++;	
	TimeDelay--;
	RC_Analy();
}

BEEP.c

#include "Beep.h"
#include "stm32f10x.h"

void BEEP_INIT(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);    //使能GPIO的时钟  
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;          //蜂鸣器
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;    //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	BEEP_L;
	
}

OLED.c

#include "oled.h"
#include "stdlib.h"
#include "oledfont.h"  	 
#include "delay.h"
#include "remote.h"

u8 OLED_GRAM[144][8];

//反显函数
void OLED_ColorTurn(u8 i)
{
	if(i==0)
		{
			OLED_WR_Byte(0xA6,OLED_CMD);//正常显示
		}
	if(i==1)
		{
			OLED_WR_Byte(0xA7,OLED_CMD);//反色显示
		}
}

//屏幕旋转180度
void OLED_DisplayTurn(u8 i)
{
	if(i==0)
		{
			OLED_WR_Byte(0xC8,OLED_CMD);//正常显示
			OLED_WR_Byte(0xA1,OLED_CMD);
		}
	if(i==1)
		{
			OLED_WR_Byte(0xC0,OLED_CMD);//反转显示
			OLED_WR_Byte(0xA0,OLED_CMD);
		}
}


void OLED_WR_Byte(u8 dat,u8 cmd)
{	
	u8 i;			  
	if(cmd)
	  OLED_DC_Set();
	else
	  OLED_DC_Clr();
	for(i=0;i<8;i++)
	{
		OLED_SCLK_Clr();
		if(dat&0x80)
		   OLED_SDIN_Set();
		else 
		   OLED_SDIN_Clr();
		OLED_SCLK_Set();
		dat<<=1;   
	}				 		  
	OLED_DC_Set();   	  
}

//开启OLED显示 
void OLED_DisPlay_On(void)
{
	OLED_WR_Byte(0x8D,OLED_CMD);//电荷泵使能
	OLED_WR_Byte(0x14,OLED_CMD);//开启电荷泵
	OLED_WR_Byte(0xAF,OLED_CMD);//点亮屏幕
}

//关闭OLED显示 
void OLED_DisPlay_Off(void)
{
	OLED_WR_Byte(0x8D,OLED_CMD);//电荷泵使能
	OLED_WR_Byte(0x10,OLED_CMD);//关闭电荷泵
	OLED_WR_Byte(0xAF,OLED_CMD);//关闭屏幕
}

//更新显存到OLED	
void OLED_Refresh(void)
{
	u8 i,n;
	for(i=0;i<8;i++)
	{
	   OLED_WR_Byte(0xb0+i,OLED_CMD); //设置行起始地址
	   OLED_WR_Byte(0x00,OLED_CMD);   //设置低列起始地址
	   OLED_WR_Byte(0x10,OLED_CMD);   //设置高列起始地址
	   for(n=0;n<128;n++)
		 OLED_WR_Byte(OLED_GRAM[n][i],OLED_DATA);
  }
}
//清屏函数
void OLED_Clear(void)
{
	u8 i,n;
	for(i=0;i<8;i++)
	{
	   for(n=0;n<128;n++)
			{
			 OLED_GRAM[n][i]=0;//清除所有数据
			}
  }
	OLED_Refresh();//更新显示
}

//画点 
//x:0~127
//y:0~63
void OLED_DrawPoint(u8 x,u8 y)
{
	u8 i,m,n;
	i=y/8;
	m=y%8;
	n=1<<m;
	OLED_GRAM[x][i]|=n;
}

//清除一个点
//x:0~127
//y:0~63
void OLED_ClearPoint(u8 x,u8 y)
{
	u8 i,m,n;
	i=y/8;
	m=y%8;
	n=1<<m;
	OLED_GRAM[x][i]=~OLED_GRAM[x][i];
	OLED_GRAM[x][i]|=n;
	OLED_GRAM[x][i]=~OLED_GRAM[x][i];
}


//画线
//x:0~128
//y:0~64
void OLED_DrawLine(u8 x1,u8 y1,u8 x2,u8 y2)
{
	u8 i,k,k1,k2;
	if((x2>128)||(y2>64)||(x1>x2)||(y1>y2))
		return;
	if(x1==x2)    //画竖线
	{
			for(i=0;i<(y2-y1);i++)
			{
				OLED_DrawPoint(x1,y1+i);
			}
  }
	else if(y1==y2)   //画横线
	{
			for(i=0;i<(x2-x1);i++)
			{
				OLED_DrawPoint(x1+i,y1);
			}
  }
	else      //画斜线
	{
		k1=y2-y1;
		k2=x2-x1;
		k=k1*10/k2;
		for(i=0;i<(x2-x1);i++)
			{
			  OLED_DrawPoint(x1+i,y1+i*k/10);
			}
	}
}
//x,y:圆心坐标
//r:圆的半径
void OLED_DrawCircle(u8 x,u8 y,u8 r)
{
	int a, b,num;
    a = 0;
    b = r;
    while(2 * b * b >= r * r)      
    {
        OLED_DrawPoint(x + a, y - b);
        OLED_DrawPoint(x - a, y - b);
        OLED_DrawPoint(x - a, y + b);
        OLED_DrawPoint(x + a, y + b);
 
        OLED_DrawPoint(x + b, y + a);
        OLED_DrawPoint(x + b, y - a);
        OLED_DrawPoint(x - b, y - a);
        OLED_DrawPoint(x - b, y + a);
        
        a++;
        num = (a * a + b * b) - r*r;//计算画的点离圆心的距离
        if(num > 0)
        {
            b--;
            a--;
        }
    }
}



//在指定位置显示一个字符,包括部分字符
//x:0~127
//y:0~63
//size:选择字体 12/16/24
//取模方式 逐列式
void OLED_ShowChar(u8 x,u8 y,u8 chr,u8 size1)
{
	u8 i,m,temp,size2,chr1;
	u8 y0=y;
	size2=(size1/8+((size1%8)?1:0))*(size1/2);  //得到字体一个字符对应点阵集所占的字节数
	chr1=chr-' ';  //计算偏移后的值
	for(i=0;i<size2;i++)
	{
		if(size1==12)
        {temp=asc2_1206[chr1][i];} //调用1206字体
		else if(size1==16)
        {temp=asc2_1608[chr1][i];} //调用1608字体
		else if(size1==24)
        {temp=asc2_2412[chr1][i];} //调用2412字体
		else return;
				for(m=0;m<8;m++)           //写入数据
				{
					if(temp&0x80)OLED_DrawPoint(x,y);
					else OLED_ClearPoint(x,y);
					temp<<=1;
					y++;
					if((y-y0)==size1)
					{
						y=y0;
						x++;
						break;
          }
				}
  }
}


//显示字符串
//x,y:起点坐标  
//size1:字体大小 
//*chr:字符串起始地址 
void OLED_ShowString(u8 x,u8 y,u8 *chr,u8 size1)
{
	while((*chr>=' ')&&(*chr<='~'))//判断是不是非法字符!
	{
		OLED_ShowChar(x,y,*chr,size1);
		x+=size1/2;
		if(x>128-size1)  //换行
		{
			x=0;
			y+=2;
    }
		chr++;
  }
}

//m^n
u32 OLED_Pow(u8 m,u8 n)
{
	u32 result=1;
	while(n--)
	{
	  result*=m;
	}
	return result;
}

////显示2个数字
////x,y :起点坐标	 
////len :数字的位数
////size:字体大小
void OLED_ShowNum(u8 x,u8 y,u32 num,u8 len,u8 size1)
{
	u8 t,temp;
	for(t=0;t<len;t++)
	{
		temp=(num/OLED_Pow(10,len-t-1))%10;
			if(temp==0)
			{
				OLED_ShowChar(x+(size1/2)*t,y,'0',size1);
      }
			else 
			{
			  OLED_ShowChar(x+(size1/2)*t,y,temp+'0',size1);
			}
  }
}

//显示汉字
//x,y:起点坐标
//num:汉字对应的序号
//取模方式 列行式
void OLED_ShowChinese(u8 x,u8 y,u8 num,u8 size1)
{
	u8 i,m,n=0,temp,chr1;
	u8 x0=x,y0=y;
	u8 size3=size1/8;
	while(size3--)
	{
		chr1=num*size1/8+n;
		n++;
			for(i=0;i<size1;i++)
			{
				if(size1==16)
						{temp=Hzk1[chr1][i];}//调用16*16字体
				else if(size1==24)
						{temp=Hzk2[chr1][i];}//调用24*24字体
				else if(size1==32)       
						{temp=Hzk3[chr1][i];}//调用32*32字体
				else if(size1==64)
						{temp=Hzk4[chr1][i];}//调用64*64字体
				else return;
							
						for(m=0;m<8;m++)
							{
								if(temp&0x01)OLED_DrawPoint(x,y);
								else OLED_ClearPoint(x,y);
								temp>>=1;
								y++;
							}
							x++;
							if((x-x0)==size1)
							{x=x0;y0=y0+8;}
							y=y0;
			 }
	}
}

//num 显示汉字的个数
//space 每一遍显示的间隔
void OLED_ScrollDisplay(u8 num,u8 space)
{
	u8 i,n,t=0,m=0,r;
	while(1)
	{
		if(m==0)
		{
	    OLED_ShowChinese(128,24,t,16); //写入一个汉字保存在OLED_GRAM[][]数组中
			t++;
		}
		if(t==num)
			{
				for(r=0;r<16*space;r++)      //显示间隔
				 {
					for(i=0;i<144;i++)
						{
							for(n=0;n<8;n++)
							{
								OLED_GRAM[i-1][n]=OLED_GRAM[i][n];
							}
						}
           OLED_Refresh();
				 }
        t=0;
      }
		m++;
		if(m==16){m=0;}
		for(i=0;i<144;i++)   //实现左移
		{
			for(n=0;n<8;n++)
			{
				OLED_GRAM[i-1][n]=OLED_GRAM[i][n];
			}
		}
		OLED_Refresh();
	}
}

//配置写入数据的起始位置
void OLED_WR_BP(u8 x,u8 y)
{
	OLED_WR_Byte(0xb0+y,OLED_CMD);//设置行起始地址
	OLED_WR_Byte(((x&0xf0)>>4)|0x10,OLED_CMD);
	OLED_WR_Byte((x&0x0f),OLED_CMD);
}

//x0,y0：起点坐标
//x1,y1：终点坐标
//BMP[]：要写入的图片数组
void OLED_ShowPicture(u8 x0,u8 y0,u8 x1,u8 y1,u8 BMP[])
{
	u32 j=0;
	u8 x=0,y=0;
	if(y%8==0)y=0;
	else y+=1;
	for(y=y0;y<y1;y++)
	 {
		 OLED_WR_BP(x0,y);
		 for(x=x0;x<x1;x++)
		 {
			 OLED_WR_Byte(BMP[j],OLED_DATA);
			 j++;
     }
	 }
}
//OLED的初始化
void OLED_Init(void)
{
	GPIO_InitTypeDef  GPIO_InitStructure;
 	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);	 //使能A端口时钟
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;
 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 		 //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//速度50MHz
 	GPIO_Init(GPIOB, &GPIO_InitStructure);	  //初始化GPIOD3,6
 	GPIO_SetBits(GPIOB,GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_8);
	
	OLED_RST_Set();
	delay_ms(100);
	OLED_RST_Clr();//复位
	delay_ms(200);
	OLED_RST_Set();
	
	OLED_WR_Byte(0xAE,OLED_CMD);//--turn off oled panel
	OLED_WR_Byte(0x00,OLED_CMD);//---set low column address
	OLED_WR_Byte(0x10,OLED_CMD);//---set high column address
	OLED_WR_Byte(0x40,OLED_CMD);//--set start line address  Set Mapping RAM Display Start Line (0x00~0x3F)
	OLED_WR_Byte(0x81,OLED_CMD);//--set contrast control register
	OLED_WR_Byte(0xCF,OLED_CMD);// Set SEG Output Current Brightness
	OLED_WR_Byte(0xA1,OLED_CMD);//--Set SEG/Column Mapping     0xa0左右反置 0xa1正常
	OLED_WR_Byte(0xC8,OLED_CMD);//Set COM/Row Scan Direction   0xc0上下反置 0xc8正常
	OLED_WR_Byte(0xA6,OLED_CMD);//--set normal display
	OLED_WR_Byte(0xA8,OLED_CMD);//--set multiplex ratio(1 to 64)
	OLED_WR_Byte(0x3f,OLED_CMD);//--1/64 duty
	OLED_WR_Byte(0xD3,OLED_CMD);//-set display offset	Shift Mapping RAM Counter (0x00~0x3F)
	OLED_WR_Byte(0x00,OLED_CMD);//-not offset
	OLED_WR_Byte(0xd5,OLED_CMD);//--set display clock divide ratio/oscillator frequency
	OLED_WR_Byte(0x80,OLED_CMD);//--set divide ratio, Set Clock as 100 Frames/Sec
	OLED_WR_Byte(0xD9,OLED_CMD);//--set pre-charge period
	OLED_WR_Byte(0xF1,OLED_CMD);//Set Pre-Charge as 15 Clocks & Discharge as 1 Clock
	OLED_WR_Byte(0xDA,OLED_CMD);//--set com pins hardware configuration
	OLED_WR_Byte(0x12,OLED_CMD);
	OLED_WR_Byte(0xDB,OLED_CMD);//--set vcomh
	OLED_WR_Byte(0x40,OLED_CMD);//Set VCOM Deselect Level
	OLED_WR_Byte(0x20,OLED_CMD);//-Set Page Addressing Mode (0x00/0x01/0x02)
	OLED_WR_Byte(0x02,OLED_CMD);//
	OLED_WR_Byte(0x8D,OLED_CMD);//--set Charge Pump enable/disable
	OLED_WR_Byte(0x14,OLED_CMD);//--set(0x10) disable
	OLED_WR_Byte(0xA4,OLED_CMD);// Disable Entire Display On (0xa4/0xa5)
	OLED_WR_Byte(0xA6,OLED_CMD);// Disable Inverse Display On (0xa6/a7) 
	OLED_WR_Byte(0xAF,OLED_CMD);
	OLED_Clear();
}
void OLED_Config(void)
{
	OLED_Init();
	OLED_ColorTurn(0);//0正常显示，1 反色显示
  OLED_DisplayTurn(0);//0正常显示 1 屏幕翻转显示
	OLED_Refresh();
}
u8 tmpp=0;
void OLED_ShowData(void)
{
	OLED_ShowString(0,0,"Thr:",12);
	OLED_ShowNum(40,0,Remote.thr,4,12);
	OLED_ShowString(0,15,"Yaw:",12);
	OLED_ShowNum(40,15,Remote.yaw,4,12);
	OLED_ShowString(0,30,"L&R:",12);
	OLED_ShowNum(40,30,Remote.pitch,4,12);
	OLED_ShowString(0,45,"F&B:",12);
	OLED_ShowNum(40,45,Remote.roll,4,12);
	
	OLED_Refresh();
}

Remote.c

#include "stm32f10x.h" 
#include "remote.h"
#include "ADC_DMA_Config.h"
#include "systick.h"
#include "nrf24l01.h"
#include <math.h>
#include "eeprom.h"
#include "delay.h"
#include "usart.h"
#include "Beep.h"

__align(4) _st_Remote Remote={1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000};


void key(void);
	
uint16_t styb;
struct //校准数据
{
	int16_t flag;		//校准标志位
	int16_t roll;
	int16_t pitch;	
	int16_t thr;	
	int16_t yaw;	
}offset ;
/******************************************************************************
函数原型：
功    能：	按键初始化
*******************************************************************************/ 
void key_init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	AFIO->MAPR = 0X02000000;   //使用四线SWD下载程序
	RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB , ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;    
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;    //上拉输入
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

	GPIO_Init(GPIOB, &GPIO_InitStructure);	

}
/******************************************************************************
函数原型：
功    能：	校准数据初始化
*******************************************************************************/ 
void RC_INIT(void)
{

	offset.flag = *(int16_t*)FLASH_Start_Addr;   //读出存储的数据
	
	if(offset.flag != 0x0066) //检查是否有校准数据 改变
	{
		delay_ms(1000);
		offset.roll = 500- (uint16_t)(0.25f*ADC_ConvertedValue[0]);//方向;
		offset.pitch = (uint16_t)(0.25f*ADC_ConvertedValue[1]) -500;//方向
		
		offset.thr = (uint16_t)(0.25f*ADC_ConvertedValue[3]);//油门;
		offset.yaw = 500- (uint16_t)(0.25f*ADC_ConvertedValue[2]);//偏航
		offset.flag = 0x0066;
    
		printf("offset thr: %d   offset pitch: %d   offset yaw: %d   offset roll: %d \r\n",offset.thr,offset.pitch,offset.yaw,offset.roll);  //串口1输出2个摇杆数据		
		
		if(offset.thr <= 1)  //判断油门打最低才存储摇杆值  不然存储了就解锁不了飞机
	  {
			FLASH_write((int16_t *)&offset,5);
		}
	}
	else
	{
			offset.roll = *((int16_t*)FLASH_Start_Addr+1);
			offset.pitch = *((int16_t*)FLASH_Start_Addr+2);
			offset.thr = *((int16_t*)FLASH_Start_Addr+3);
			offset.yaw = *((int16_t*)FLASH_Start_Addr+4);
		
		  
	}
	key_init();
}
/******************************************************************************
函数原型：
功    能：	10ms发送一次数据，每个通道都是高位在前低位在后
通信协议：
起始帧0XAA,0XAF + 地址帧 0X03 + 数据长度 + 12个遥控数据 + 校验字节
*******************************************************************************/ 
uint8_t tx_data[32] = {0xAA,0xAF,0x03,32-5};//匿名通信协议
void NRF_SEND(void)
{
	for(uint8_t i=4;i<26;i+=2)
	{
		tx_data[i] = *((uint8_t*)&Remote+i-3);	 //高位在前
		tx_data[31] += tx_data[i];
		tx_data[i+1] = *((uint8_t*)&Remote+i-4);	
		tx_data[31] += tx_data[i];
	}
	tx_data[31] = 0;
		for(uint8_t i=0;i<31;i++)  //校验位
		{
			tx_data[31] +=  tx_data[i];
		}
	NRF24L01_TxPacket((uint8_t*)&tx_data); //调用NRF发射数据
}
/******************************************************************************
函数原型：
功    能：	遥控数据解析   左油门程序
*******************************************************************************/ 
void RC_Analy(void) //10ms调用一次
{
	static uint16_t last_thr,last_roll,last_pitch,last_yaw;

	Remote.thr = (uint16_t)(0.25f*ADC_ConvertedValue[3])+1000 + offset.thr;//油门
	last_thr = Remote.thr = Remote.thr*0.25f + 0.75f*last_thr;
	Remote.pitch 	= 	1000 + (uint16_t)(0.25f*ADC_ConvertedValue[1]) + offset.pitch;//
	last_pitch = Remote.pitch = Remote.pitch*0.25f + 0.75f*last_pitch;
	Remote.yaw 		= 	1000 + (uint16_t)(0.25f*ADC_ConvertedValue[2]) + offset.yaw;//方向
			if(Remote.yaw>1500)		//把大数转换为小数
		{
			if(Remote.yaw>1999)
			{
			  Remote.yaw=1005;
			}
			else 
			{
				Remote.yaw=Remote.yaw-1500;
				Remote.yaw=1500-Remote.yaw;
			}
		}
		else if(Remote.yaw<1500)	//把小数转换为大数
		{
			if(Remote.yaw<1010)
			{
				Remote.yaw=2000;
			}
			else
			{
			   Remote.yaw=1500-Remote.yaw;
			   Remote.yaw=1500+Remote.yaw;
			}	 
		}
	last_yaw = Remote.yaw = Remote.yaw*0.25f + 0.75f*last_yaw;
	Remote.roll 	=	1000 +(uint16_t)(0.25f*ADC_ConvertedValue[0]) + offset.roll;//副翼
		if(Remote.roll>1500)		//把大数转换为小数
		{
			if(Remote.roll>1999)
			{
			  Remote.roll=1005;
			}
			else 
			{
				Remote.roll=Remote.roll-1500;
				Remote.roll=1500-Remote.roll;
			}
		}
		else if(Remote.roll<1500)	//把小数转换为大数
		{
			if(Remote.roll<1010)
			{
				Remote.roll=2000;
			}
			else
			{
			   Remote.roll=1500-Remote.roll;
			   Remote.roll=1500+Remote.roll;
			}	 
		}
	last_roll = Remote.roll = Remote.roll*0.25f + 0.75f*last_roll;
	styb++;
	if(styb >= 200)
	{	
		printf("前后: %d   左右: %d   航向: %d   油门: %d \r\n",Remote.pitch,Remote.roll,Remote.yaw,Remote.thr);  //串口1输出2个摇杆数据
		styb=0;
	}
	key();//扫描按键
	NRF_SEND();
}


/******************************************************************************
函数原型：
功    能：	按键扫描
*******************************************************************************/ 
void key(void)
{
	#define KEY3 GPIO_Pin_3//GPIOC
	#define KEY4 GPIO_Pin_4//GPIOC
	#define KEY5 GPIO_Pin_5 //GPIOA
	#define KEY6 GPIO_Pin_6 //GPIOA
	#define KEY7 GPIO_Pin_7 //GPIOB
	#define KEY8 GPIO_Pin_8 //GPIOB
  volatile static uint8_t status = 0;	
 	static uint32_t temp;
	switch(status)
	{
		case 0:
			if(SysTick_count - temp >20) //200ms 按键音
			{
				if(
					((GPIOB->IDR & (GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8)) == (GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8))					
					)
					status = 1;
				BEEP_H;
			  //GPIO_SetBits(GPIOB,GPIO_Pin_10);
				GPIO_ResetBits(GPIOB,GPIO_Pin_10);
			}
			break;
		case 1:
			  if(
					((GPIOB->IDR & (GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8)) != (GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8))					
					)
				status = 2;
			break;
		case 2:	
				if(!(GPIOB->IDR & KEY6))
			{
					offset.roll=offset.roll+5;	
			}
			else if(!(GPIOB->IDR & KEY4))
			{
					offset.roll=offset.roll-5;
			}
			else if(!(GPIOB->IDR & KEY5))
			{
					offset.pitch=offset.pitch-5;	
			}
			else if(!(GPIOB->IDR & KEY3))
			{
					offset.pitch=offset.pitch+5;
			}
			else if(!(GPIOB->IDR & KEY7))
			{
				offset.thr = 0;	
				FLASH_write((int16_t *)&offset,5);	
			}
			else if(!(GPIOB->IDR & KEY8))
			{
					Remote.AUX7 ^= (2000^1000); //1000和2000之间变化	  可以留来一键空翻
			}	
			status = 0;			
			BEEP_L;  
			//GPIO_ResetBits(GPIOB,GPIO_Pin_10);
			GPIO_SetBits(GPIOB,GPIO_Pin_10);
			temp = SysTick_count;
			break;
		}			
}

delay.c

#include "stm32f10x.h"
#include "misc.h"
#include "delay.h"
#include "systick.h"

static volatile uint32_t usTicks = 0;
uint16_t TimeDelay=0;
void cycleCounterInit(void)
{
    RCC_ClocksTypeDef clocks;
    RCC_GetClocksFreq(&clocks);
    usTicks = clocks.SYSCLK_Frequency / 1000000;
}

void delay_ms(uint16_t nTime) //    毫秒级延时函数	 
{
  TimeDelay=nTime;
	while(TimeDelay);
}

Usart.c

#include "usart.h"
#include "misc.h"
#include "stdio.h"

/*
 * 函数名：USART1_Config
 * 描述  ：USART1 GPIO 配置,工作模式配置。
 * 输入  ：无
 * 输出  : 无
 * 调用  ：外部调用
 */
void USART1_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure; 
	/* 配置串口1 （USART1） 时钟*/
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOA, ENABLE);
	
	/* Configure the NVIC Preemption Priority Bits */  

	
	/* 使能串口1中断 */
	NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;	//USART1  串口1全局中断 
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;//抢占优先级1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; //子优先级1
	/*IRQ通道使能*/
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	/*根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器USART1*/
	NVIC_Init(&NVIC_InitStructure);
	
	

	/*串口GPIO端口配置*/
  /* 配置串口1 （USART1 Tx (PA.09)）*/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
  
	/* 配置串口1 USART1 Rx (PA.10)*/
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	
	
	//USART 初始化设置
	USART_InitStructure.USART_BaudRate =115200;//串口波特率
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
	USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
	USART_InitStructure.USART_Mode = USART_Mode_Tx;//|USART_Mode_Rx | ;	//发模式
	USART_Init(USART1, &USART_InitStructure);
	
//	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启中断
	
	USART_Cmd(USART1, ENABLE); //使能串口 
}




void USART1_SendByte(const int8_t *Data,uint8_t len)
{ 
	uint8_t i;
	
	for(i=0;i<len;i++)
	{
		while (!(USART1->SR & USART_FLAG_TXE));	 // while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
		USART_SendData(USART1,*(Data+i));	 
	}
}

void USART3_SendByte(const int8_t *Data,uint8_t len)
{ 
	uint8_t i;
	
	for(i=0;i<len;i++)
	{
		while (!(USART3->SR & USART_FLAG_TXE));	 // while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
		USART_SendData(USART3,*(Data+i));	 
	}
}


int8_t CheckSend[7]={0xAA,0XAA,0xEF,2,0,0,0};

void USART1_setBaudRate(uint32_t baudRate)
{
	USART_InitTypeDef USART_InitStructure;
	USART_InitStructure.USART_BaudRate =  baudRate;
	USART_Init(USART1, &USART_InitStructure);
}






/*
 * 函数名：fputc
 * 描述  ：重定向c库函数printf到USART1
 * 输入  ：无
 * 输出  ：无
 * 调用  ：由printf调用
 */
int fputc(int ch, FILE *f)
{
	/* 将Printf内容发往串口 */
	USART_SendData(USART1, (unsigned char) ch);
	while( USART_GetFlagStatus(USART1,USART_FLAG_TC)!= SET);	
	return (ch);
}