stm32两轮平衡小车- 02

阿容1234562025-12-03 9:25

代码分享

iic

cs 复制代码 
#include "iic.h"
#include "systick.h"
#include "imath.h"

//IIC初始化
void IIC_Init(void)
{
    GPIO_InitTypeDef  GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(IIC_RCC_port, ENABLE);
    
    GPIO_InitStructure.GPIO_Pin    = SCL_pin;
    GPIO_InitStructure.GPIO_Mode   = GPIO_Mode_Out_OD;                 //开漏输出    
    GPIO_InitStructure.GPIO_Speed  = GPIO_Speed_50MHz;
    GPIO_Init(SCL_port, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin    = SDA_pin;
    GPIO_InitStructure.GPIO_Mode   = GPIO_Mode_Out_OD;                 //开漏输出    
    GPIO_InitStructure.GPIO_Speed  = GPIO_Speed_50MHz;
    GPIO_Init(SDA_port, &GPIO_InitStructure);
    
    tdelay_ms(10);
    GPIO_SetBits(SCL_port,SCL_pin);                                    //上拉
    GPIO_SetBits(SDA_port,SDA_pin);                                    //上拉
}
//数据端口配置为输出模式   
void SDA_OUT(void)
{
	GPIO_InitTypeDef  GPIO_InitStructure;
    
	GPIO_InitStructure.GPIO_Pin   = SDA_pin ;
    GPIO_InitStructure.GPIO_Mode   = GPIO_Mode_Out_PP;                 //输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(SDA_port, &GPIO_InitStructure);
}
//数据端口配置为输入模式   
void SDA_IN(void)
{
	GPIO_InitTypeDef  GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Pin  = SDA_pin ;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;

	GPIO_Init(SDA_port, &GPIO_InitStructure);
}
//iic延时使用
static void IIC_delay(void)
{
//    volatile int i = 1;	
//    while (i)
//        i--;
}
//起始信号  
void IIC_Start(void)
{
	SDA_OUT();
	SCL_HIGH;
	SDA_HIGH;
	IIC_delay();
	SDA_LOW;
	IIC_delay();
}
//停止信号    
void IIC_Stop(void)
{
	SDA_OUT();
	SCL_LOW;
	IIC_delay();
	SDA_LOW;
	IIC_delay();
	SCL_HIGH;
	IIC_delay();
	SDA_HIGH;
	IIC_delay();
}
/* 等待从机应答信号          */
/* 返回值： 0：接收应答失败  */
/*          1：接收应答成功  */
unsigned char IIC_Slave_Ack(void)
{
	SDA_OUT();
	SCL_LOW;
	IIC_delay();
	SDA_HIGH;

	SDA_IN();
	IIC_delay();
	SCL_HIGH;

	IIC_delay();

	if(SDA_READ)
	{
		SCL_LOW;
		return 0;
	}
	SCL_LOW;
	IIC_delay();
	return 1;
}

//发送一个字节   
void IIC_Send_Byte(unsigned char byte)
{
    unsigned char i = 8;
    SDA_OUT();
    while (i--)
    {
        SCL_LOW;                //拉低时钟开始数据传输
        IIC_delay();
        if (byte & 0x80)
            SDA_HIGH;
        else
            SDA_LOW;
        byte <<= 1;
        IIC_delay();
        SCL_HIGH;
        IIC_delay();
    }
    SCL_LOW;
    if(IIC_Slave_Ack()==0)
    {
        return ;
    }
}
/* 读取一个字节数据   */
/* 返回值：读取的数据 */
unsigned char IIC_Read_Byte(void)
{
    unsigned char i;
    unsigned char dat = 0;
    SDA_IN();
    for (i=0; i<8; i++)
    {
        dat <<= 1;
        SCL_HIGH;
        IIC_delay();
        dat |= SDA_READ;
        SCL_LOW;
        IIC_delay();
    }
    return dat;
}

/* 向寄存器写一个字节数据       */
/* SlaveAddress：从机地址       */
/* REG_Address：寄存器地址      */
/* REG_data：向寄存器写入的数据 */
void IIC_Write_One_Byte(unsigned char SlaveAddress,unsigned char REG_Address,unsigned char REG_data)
{
    IIC_Start();
    IIC_Send_Byte(SlaveAddress);
    IIC_Send_Byte(REG_Address);
    IIC_Send_Byte(REG_data);
    IIC_Stop();
}

/* 从寄存器读取一个字节数据 */
/* SlaveAddress：从机地址   */
/* REG_Address： 寄存器地址 */
/* 返回值：读取的数据       */
unsigned char IIC_Read_One_Byte(unsigned char SlaveAddress,unsigned char REG_Address)
{
	unsigned char REG_data;
    
	IIC_Start();
	IIC_Send_Byte(SlaveAddress);
	IIC_Send_Byte(REG_Address);
	IIC_Start();
	IIC_Send_Byte(SlaveAddress+1);
	REG_data = IIC_Read_Byte();
	IIC_Slave_Ack();
	IIC_Stop();
	return REG_data;
}
//连续读取两个字节数据   
unsigned short int IIC_Read_Two_Bytes(unsigned char SlaveAddress,unsigned char REG_Address)
{
	unsigned char H, L;
	H = IIC_Read_One_Byte(SlaveAddress,REG_Address);
	L = IIC_Read_One_Byte(SlaveAddress,REG_Address+1);
	return ( ((unsigned short int)H) << 8 | L) ;
}

串口

cs 复制代码 
#include "usart1.h"
#include "mpu6050.h"
#include "imu.h"
#include "imath.h"
#include "systick.h"
#include "key.h"
#include "pwm.h"
#include "pid.h"
#include "controller.h"
#include "timer.h"
#include "flash.h"
#include "led.h"
#include "bluetooth.h"

uint8_t data_to_send[100];                  //发送数据缓存
uint8_t USART_RX_DATA[USART_RX_LEN];        //接收数据缓存
dt_flag_t f;					            //需要发送数据的标志 

#pragma import(__use_no_semihosting)
 
struct __FILE
{
	int a;
};
 
FILE __stdout;
 
void _sys_exit(int x)
{
	
}
int fputc(int ch,FILE *f)
{
    USART1->SR; 
    USART_SendData(USART1, (unsigned char) ch);
    while(USART_GetFlagStatus(USART1,USART_FLAG_TC)!=SET);
    return(ch);
} 
/* 串口1初始化设置 */
/* 入口参数：波特率 */
void usart1_init(u32 bound)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_USART1, ENABLE);
    GPIO_InitTypeDef GPIO_InitStructure;

    //USART1 Tx(PA.09) 
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; 
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    
    //USART1 Rx(PA.10) 
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; 
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    
    USART_InitTypeDef USART_InitStructure;
    USART_InitStructure.USART_BaudRate = bound; 
    USART_InitStructure.USART_WordLength = USART_WordLength_8b; 
    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);                                    //rx interrupt is enable

    USART_Cmd(USART1, ENABLE);   
}
/* 串口DMA通道配置                */
/* DMA_CHx:         DMA传输通道x     */
/* peripheral_addr: 外设地址         */
/* memory_addr:     内存地址         */
/* data_length:     传输的数据长度   */  
void USARTx_DMA_TX_Config(DMA_Channel_TypeDef* DMA_CHx,u32 peripheral_addr,u32 memory_addr,u16 data_length)
{
    DMA_InitTypeDef DMA_InitStructure;
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);	                        //时钟使能                                                                              
    DMA_DeInit(DMA_CHx);                                                        //复位
                                                                                
    DMA_InitStructure.DMA_PeripheralBaseAddr = peripheral_addr;                 //外设地址     
    DMA_InitStructure.DMA_MemoryBaseAddr =memory_addr;                          //内存地址  
    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;                          //外设作为传输的目的地
    DMA_InitStructure.DMA_BufferSize = data_length;                             //数据缓存大小                       
    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;            //外设地址不自增
    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;                     //内存地址自增   
    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;     //外设数据宽度8位
    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;             //内存数据宽度8位
    DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;                               //正常模式
    DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;                     //高优先级
    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;                                //无内存到内存传输                    
    DMA_Init(DMA_CHx, &DMA_InitStructure);                                 
}

/* 串口DMA数据发送 */
void USARTx_DMA_SEND_DATA(u32 SendBuff,u16 len) 
{
	USARTx_DMA_TX_Config(DMA1_Channel4,(u32)&USART1->DR,(u32)SendBuff,len);
	USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);                              //使能串口DMA发送
    DMA_Cmd(DMA1_Channel4, ENABLE);                                             //使能DMA传输
}

/* 将大于一个字节的数据拆分成多个字节发送 */
#define BYTE0(dwTemp)       ( *( (char *)(&dwTemp)    ) )
#define BYTE1(dwTemp)       ( *( (char *)(&dwTemp) + 1) )
#define BYTE2(dwTemp)       ( *( (char *)(&dwTemp) + 2) )
#define BYTE3(dwTemp)       ( *( (char *)(&dwTemp) + 3) )

/* 向匿名上位机发送姿态角，锁定状态 */
void ANO_DT_Send_Status(void)
{
	u8 _cnt=0;
	vs16 _temp;
	vs32 _temp2;
	u8 sum = 0;
	u8 i;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x01;
	data_to_send[_cnt++]=0;

	_temp = (int)(att.rol*100);                     //横滚角
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
    
	_temp = (int)(att.pit*100);                     //俯仰角    
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
    
	_temp = (int)(att.yaw*100);                     //偏航角
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);

	_temp2 = (int32_t)(100*99.99);       		    //高度 
	data_to_send[_cnt++]=BYTE3(_temp2);
	data_to_send[_cnt++]=BYTE2(_temp2);
	data_to_send[_cnt++]=BYTE1(_temp2);
	data_to_send[_cnt++]=BYTE0(_temp2);

    data_to_send[_cnt++]=0x01;  					//飞行模式    01：姿态  02：定高  03：定点
    data_to_send[_cnt++]=0x01;                      //锁定状态

	data_to_send[3] = _cnt-4;
	sum = 0;
	for(i=0;i<_cnt;i++)
		sum += data_to_send[i];
	data_to_send[_cnt++]=sum;
    
    USARTx_DMA_SEND_DATA((u32)(data_to_send),_cnt); //发送           
}

 void ANO_DT_Send_Senser(s16 a_x,s16 a_y,s16 a_z,s16 g_x,s16 g_y,s16 g_z,s16 m_x,s16 m_y,s16 m_z)
{
    u8 _cnt=0;
    vs16 _temp;
    u8 sum = 0;
    u8 i=0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x02;
    data_to_send[_cnt++]=0;

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

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

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

    data_to_send[3] = _cnt-4;

    sum = 0;
    for(i=0;i<_cnt;i++)
        sum += data_to_send[i];
    data_to_send[_cnt++] = sum;
    
    USARTx_DMA_SEND_DATA((u32)(data_to_send),_cnt);
}

/* 遥控器通道数据 */
void ANO_DT_Send_RCData(u16 thr,u16 yaw,u16 rol,u16 pit,u16 aux1,u16 aux2,u16 aux3,u16 aux4,u16 aux5,u16 aux6)
{
    u8 _cnt=0;
    u8 i=0;
    u8 sum = 0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x03;
    data_to_send[_cnt++]=0;
    
    data_to_send[_cnt++]=BYTE1(thr);
    data_to_send[_cnt++]=BYTE0(thr);
    data_to_send[_cnt++]=BYTE1(yaw);
    data_to_send[_cnt++]=BYTE0(yaw);
    data_to_send[_cnt++]=BYTE1(rol);
    data_to_send[_cnt++]=BYTE0(rol);
    data_to_send[_cnt++]=BYTE1(pit);
    data_to_send[_cnt++]=BYTE0(pit);
    
    data_to_send[_cnt++]=BYTE1(aux1);
    data_to_send[_cnt++]=BYTE0(aux1);
    data_to_send[_cnt++]=BYTE1(aux2);
    data_to_send[_cnt++]=BYTE0(aux2);
    data_to_send[_cnt++]=BYTE1(aux3);
    data_to_send[_cnt++]=BYTE0(aux3);
    data_to_send[_cnt++]=BYTE1(aux4);
    data_to_send[_cnt++]=BYTE0(aux4);
    data_to_send[_cnt++]=BYTE1(aux5);
    data_to_send[_cnt++]=BYTE0(aux5);
    data_to_send[_cnt++]=BYTE1(aux6);
    data_to_send[_cnt++]=BYTE0(aux6);

    data_to_send[3] = _cnt-4;

    sum = 0;
    for(i=0;i<_cnt;i++)
        sum += data_to_send[i];
    data_to_send[_cnt++]=sum;
    
    USARTx_DMA_SEND_DATA((u32)(data_to_send),_cnt);
}

void ANO_DT_Send_Power(float votage, float current)
{
	u8 _cnt=0;
	u16 temp;
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x05;
	data_to_send[_cnt++]=0;
	
	temp = (uint16_t)100*votage;
	data_to_send[_cnt++]=BYTE1(temp);
	data_to_send[_cnt++]=BYTE0(temp);
	temp = (uint16_t)100*current;
	data_to_send[_cnt++]=BYTE1(temp);
	data_to_send[_cnt++]=BYTE0(temp);
	
	data_to_send[3] = _cnt-4;
	
	u8 sum = 0;
	for(u8 i=0;i<_cnt;i++)
		sum += data_to_send[i];
	data_to_send[_cnt++]=sum;
	
	USARTx_DMA_SEND_DATA((u32)(data_to_send), _cnt);
}
//用户自定义数据发送：1-5：int16t类型数据     6-10：float类型数据
 void ANO_DT_Send_User(s16 user1,s16 user2,s16 user3,s16 user4,s16 user5,
                        float user6, float user7, float user8, float user9, float user10,
                        float user11,float user12,float user13,float user14,float user15)
{
    u8 _cnt=0;
    vs16 _temp;
    float _temp_f;
    
    u8 sum = 0;
    u8 i=0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xF1;
    data_to_send[_cnt++]=0;
    
    //1-5  int16t类型数据
    _temp = user1;    
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = user2;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = user3;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = user4;    
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = user5;
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);

    //6-10 ：float类型数据
    _temp_f = user6;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);
    _temp_f = user7;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);
    _temp_f = user8;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);
    _temp_f = user9;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);
    _temp_f = user10;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);

    _temp_f = user11;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);
    _temp_f = user12;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);
    _temp_f = user13;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);
    _temp_f = user14;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);
    _temp_f = user15;
	data_to_send[_cnt++]=BYTE3(_temp_f);
	data_to_send[_cnt++]=BYTE2(_temp_f);
	data_to_send[_cnt++]=BYTE1(_temp_f);
	data_to_send[_cnt++]=BYTE0(_temp_f);

    data_to_send[3] = _cnt-4;

    sum = 0;
    for(i=0;i<_cnt;i++)
        sum += data_to_send[i];
    data_to_send[_cnt++] = sum;
    
    USARTx_DMA_SEND_DATA((u32)(data_to_send),_cnt);
}

void ANO_DT_Send_PID(u8 group,float p1_p,float p1_i,float p1_d,float p2_p,float p2_i,float p2_d,float p3_p,float p3_i,float p3_d)
{
	u8 _cnt=0;
	vs16 _temp;
	
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x10+group-1;
	data_to_send[_cnt++]=0;

	_temp = p1_p * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p1_i  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p1_d  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p2_p  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p2_i  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p2_d * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p3_p  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p3_i  * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = p3_d * 1000;
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	
	data_to_send[3] = _cnt-4;
	
	u8 sum = 0;
	for(u8 i=0;i<_cnt;i++)
		sum += data_to_send[i];
	data_to_send[_cnt++]=sum;

	USARTx_DMA_SEND_DATA((u32)(data_to_send),_cnt);
}
/*
在上位机对下位机进行操作时，下位机做出对应的反馈信息
 MSG_ID:                 MSG_DATA:
01:加速度               01:校准成功
02:陀螺仪               E1:校准失败
03:罗盘                 31:设置成功
30:无线定位模块         32:设置成功2
40:匿名数传             A1:恢复默认成功
 */
static void ANO_DT_Send_MSG(u8 MSG_ID, u8 MSG_DATA)
{
	data_to_send[0]=0xAA;
	data_to_send[1]=0xAA;
	data_to_send[2]=0xEE;
	data_to_send[3]=2;                      //长度
	data_to_send[4]=MSG_ID;                 //功能字
	data_to_send[5]=MSG_DATA;
	
	u8 sum = 0;
	for(u8 i=0;i<6;i++)
		sum += data_to_send[i];
	data_to_send[6]=sum;

	USARTx_DMA_SEND_DATA((u32)(data_to_send),7);
}

void ANO_DMA_SEND_DATA(void)
{
    static uint8_t ANO_debug_cnt = 0;
    ANO_debug_cnt++;
    if(ANO_debug_cnt==1)
    {
        ANO_DT_Send_Status();
    }
    else if(ANO_debug_cnt==2)
    { 
        ANO_DT_Send_Senser((int16_t)acc_raw.x,(int16_t)acc_raw.y,(int16_t)acc_raw.z, 
                           (int16_t)gyro_raw.x,(int16_t)gyro_raw.y,(int16_t)gyro_raw.z,
                           (int16_t)0,(int16_t)0,(int16_t)0);
    }
    else if(ANO_debug_cnt==3)
    {
        ANO_DT_Send_RCData(1100,1200,1300,1400,1500,1600,1700,1800,1900,1100);
    }
    else if(ANO_debug_cnt==4)
    {
        ANO_DT_Send_Power(13.52,57.63);
    }
    else if(ANO_debug_cnt==5)
    {
        ANO_DT_Send_User(0,0,0,0,0,
                        encoder_val_a, encoder_val_b,ctr.pwm[0],ctr.pwm[1],acc_raw_f.z,
                        0,0,0,0,0);
    }
    else if(ANO_debug_cnt==6)
    {
        if(f.send_pid1)
        {
            f.send_pid1 = 0;
            //向上位机发送内环速度环PID参数值
            ANO_DT_Send_PID(1,  vel.kp*0.1f,
                                vel.ki,
                                vel.kd,
                                1.5f,
                                2.5f,
                                1.5f,
                                2.5f,
                                1.5f,
                                2.5f);        
        }
        if(f.send_pid2)
        {
            f.send_pid2 = 0;
            //向上位机发送外环角度环PID参数值
            ANO_DT_Send_PID(2,  bal.kp*0.1f,
                                bal.ki,
                                bal.kd,
                                2.6f,
                                1.5f,
                                2.4f,
                                1.1f,
                                2.2f,
                                1.1f);   
        }
        if(CalGyro.success==1)                          //返回校准成功信息给上位机
        {
            CalGyro.success = 0;
            ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);
        }
        else if(CalGyro.success==2)                     //反馈校准失败信息给上位机
        {
            CalGyro.success = 0;
            ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);
        }
    }
    else if(ANO_debug_cnt==7)
    {
        ANO_debug_cnt = 0;
    }
}
static void ANO_DT_Send_Check(u8 head, u8 check_sum)
{
	data_to_send[0]=0xAA;
	data_to_send[1]=0xAA;
	data_to_send[2]=0xEF;
	data_to_send[3]=2;                  //长度
	data_to_send[4]=head;               //功能字
	data_to_send[5]=check_sum;
	
	u8 sum = 0;
	for(u8 i=0;i<6;i++)
		sum += data_to_send[i];
	data_to_send[6]=sum;

	USARTx_DMA_SEND_DATA((u32)(data_to_send),7);
}

//Data_Receive_Prepare函数是协议预解析，根据协议的格式，将收到的数据进行一次格式性解析，格式正确的话再进行数据解析
//移植时，此函数应由用户根据自身使用的通信方式自行调用，比如串口每收到一字节数据，则调用此函数一次
//此函数解析出符合格式的数据帧后，会自行调用数据解析函数
void ANO_DT_Data_Receive_Prepare(u8 data)
{
	static u8 RxBuffer[50];
	static u8 _data_len = 0,_data_cnt = 0;
	static u8 state = 0;
	
	if(state==0&&data==0xAA)
	{
		state=1;
		RxBuffer[0]=data;
	}
	else if(state==1&&data==0xAF)
	{
		state=2;
		RxBuffer[1]=data;
	}
	else if(state==2&&data<0XF1)
	{
		state=3;
		RxBuffer[2]=data;
	}
	else if(state==3&&data<50)
	{
		state = 4;
		RxBuffer[3]=data;
		_data_len = data;
		_data_cnt = 0;
	}
	else if(state==4&&_data_len>0)
	{
		_data_len--;
		RxBuffer[4+_data_cnt++]=data;
		if(_data_len==0)
			state = 5;
	}
	else if(state==5)
	{
		state = 0;
		RxBuffer[4+_data_cnt]=data;
		ANO_DT_Data_Receive_Anl(RxBuffer,_data_cnt+5);
	}
	else
		state = 0;
}
 
//Data_Receive_Anl函数是协议数据解析函数，函数参数是符合协议格式的一个数据帧，该函数会首先对协议数据进行校验
//校验通过后对数据进行解析，实现相应功能
//此函数可以不用用户自行调用，由函数Data_Receive_Prepare自动调用
void ANO_DT_Data_Receive_Anl(u8 *data_buf,u8 num)
{  
	u8 sum = 0;
	for(u8 i=0;i<(num-1);i++)
    {
		sum += *(data_buf+i);        
    }
	if(!(sum==*(data_buf+num-1)))	                //判断sum	
        return;		
	if(!(*(data_buf)==0xAA && *(data_buf+1)==0xAF))	//判断帧头	
        return;		
	
	if(*(data_buf+2)==0X01)                         //功能字01
	{
//        pu8printf("buf:",data_buf,num,1);
		if(*(data_buf+4)==0X01)                     //ACC校准
        {
        }            
//			mpu6050.Acc_CALIBRATE = 1;
		if(*(data_buf+4)==0X02)                     //GYRO校准
        {
            CalGyro.flag = 1;                       //接收到校准指令后置为1    
            _led.sta = 2;                           //默认无操作
        }
		if(*(data_buf+4)==0X03)
		{
//			mpu6050.Acc_CALIBRATE = 1;		
//			mpu6050.Gyro_CALIBRATE = 1;			
		}
	}
	if(*(data_buf+2)==0X02)                         //功能字02
	{
		if(*(data_buf+4)==0X01)                     //上位机读取pid请求
		{
			f.send_pid1 = 1;
			f.send_pid2 = 1;
			f.send_pid3 = 1;
			f.send_pid4 = 1;
			f.send_pid5 = 1;
			f.send_pid6 = 1;
//            ANO_DT_Send_Check(*(data_buf+2),sum);
		}
		if(*(data_buf+4)==0X02)     //读取飞行模式设置请求
		{
		}
		if(*(data_buf+4)==0XA0)		//读取版本信息
		{
			f.send_version = 1;
		}
		if(*(data_buf+4)==0XA1)		//恢复默认参数
		{
//            PidToFactorySetup();    //参数赋值
            ANO_DT_Send_Check(*(data_buf+2),sum);
            /* 将出厂内环PID参数写入flash */
            FlashWriteNineFloat(PID_Group1_ADDRESS, WriteFlashVel.kp,WriteFlashVel.ki,WriteFlashVel.kd,
                                                    WriteFlashBal.kp,WriteFlashBal.ki,WriteFlashBal.kd,
                                                    WriteFlashTur.kp,WriteFlashTur.ki,WriteFlashTur.kd);
            /* 将出厂外环PID参数写入flash */
//            FlashWriteNineFloat(PID_Group2_ADDRESS, all.rol_angle.kp,all.rol_angle.ki,all.rol_angle.kd,
//                                                    all.pit_angle.kp,all.pit_angle.ki,all.pit_angle.kd,
//                                                    all.yaw_angle.kp,all.yaw_angle.ki,all.yaw_angle.kd);
            flash_flag.pid = 0;
            FlashWriteByte(PID_FLAG_ADDRESS,flash_flag.pid);
		}
	}
	if(*(data_buf+2)==0X10)         //设置PID组1
    {
        /* PID内环速度环参数赋值 */
//        all.rol_gyro.kp = 0.001*( (vs16)(*(data_buf+4)<<8)|*(data_buf+5) );
//        all.rol_gyro.ki = 0.001*( (vs16)(*(data_buf+6)<<8)|*(data_buf+7) );
//        all.rol_gyro.kd = 0.001*( (vs16)(*(data_buf+8)<<8)|*(data_buf+9) );
//        all.pit_gyro.kp = 0.001*( (vs16)(*(data_buf+10)<<8)|*(data_buf+11) );
//        all.pit_gyro.ki = 0.001*( (vs16)(*(data_buf+12)<<8)|*(data_buf+13) );
//        all.pit_gyro.kd = 0.001*( (vs16)(*(data_buf+14)<<8)|*(data_buf+15) );
//        all.yaw_gyro.kp	= 0.001*( (vs16)(*(data_buf+16)<<8)|*(data_buf+17) );
//        all.yaw_gyro.ki = 0.001*( (vs16)(*(data_buf+18)<<8)|*(data_buf+19) );
//        all.yaw_gyro.kd = 0.001*( (vs16)(*(data_buf+20)<<8)|*(data_buf+21) );
        ANO_DT_Send_Check(*(data_buf+2),sum);
        /* 将参数写入flash中存储 */
//        FlashWriteNineFloat(PID_Group1_ADDRESS, all.rol_gyro.kp,all.rol_gyro.ki,all.rol_gyro.kd,
//                                                all.pit_gyro.kp,all.pit_gyro.ki,all.pit_gyro.kd,
//                                                all.yaw_gyro.kp,all.yaw_gyro.ki,all.yaw_gyro.kd);
        flash_flag.pid = 1;
        /* 将标志位写入内存 */
        FlashWriteByte(PID_FLAG_ADDRESS,flash_flag.pid);
    }
    if(*(data_buf+2)==0X11)         //设置PID组2
    {
        /* PID外环角度环参数赋值 */
//        all.rol_angle.kp = 0.001*( (vs16)(*(data_buf+4)<<8)|*(data_buf+5) );
//        all.rol_angle.ki = 0.001*( (vs16)(*(data_buf+6)<<8)|*(data_buf+7) );
//        all.rol_angle.kd = 0.001*( (vs16)(*(data_buf+8)<<8)|*(data_buf+9) );
//        all.pit_angle.kp = 0.001*( (vs16)(*(data_buf+10)<<8)|*(data_buf+11) );
//        all.pit_angle.ki = 0.001*( (vs16)(*(data_buf+12)<<8)|*(data_buf+13) );
//        all.pit_angle.kd = 0.001*( (vs16)(*(data_buf+14)<<8)|*(data_buf+15) );
//        all.yaw_angle.kp = 0.001*( (vs16)(*(data_buf+16)<<8)|*(data_buf+17) );
//        all.yaw_angle.ki = 0.001*( (vs16)(*(data_buf+18)<<8)|*(data_buf+19) );
//        all.yaw_angle.kd = 0.001*( (vs16)(*(data_buf+20)<<8)|*(data_buf+21) );
        ANO_DT_Send_Check(*(data_buf+2),sum);
        /* 将参数写入flash中存储 */
//        FlashWriteNineFloat(PID_Group2_ADDRESS, all.rol_angle.kp,all.rol_angle.ki,all.rol_angle.kd,
//                                                all.pit_angle.kp,all.pit_angle.ki,all.pit_angle.kd,
//                                                all.yaw_angle.kp,all.yaw_angle.ki,all.yaw_angle.kd);
        flash_flag.pid = 1;
        /* 将标志位写入内存 */
        FlashWriteByte(PID_FLAG_ADDRESS,flash_flag.pid);
    }
    if(*(data_buf+2)==0X12)         //设置PID组3
    {	
        ANO_DT_Send_Check(*(data_buf+2),sum);
    }
	if(*(data_buf+2)==0X13)         //设置PID组4
	{
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}
	if(*(data_buf+2)==0X14)         //设置PID组5
	{
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}
	if(*(data_buf+2)==0X15)         //设置PID组6
	{
		ANO_DT_Send_Check(*(data_buf+2),sum);
	}
}

uint16_t bufIndex = 0;
//调试串口接收中断     
void USART1_IRQHandler(void)                                 
{      
    u8 rcANO_DT;
    if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  
    {  
        if(bufIndex>USART_RX_LEN)
            bufIndex = 0;
		USART_RX_DATA[bufIndex++] = USART_ReceiveData(USART1);
        rcANO_DT = USART_ReceiveData(USART1);
        ANO_DT_Data_Receive_Prepare(rcANO_DT);
        
        USART_ClearITPendingBit(USART1, USART_IT_RXNE);                             //清除空闲中断标志         
    }   
}

蓝牙：

cs 复制代码 
#include "bluetooth.h"
#include "usart1.h"
#include "mpu6050.h"
#include "imu.h"
#include "imath.h"
#include "systick.h"
#include "key.h"
#include "pwm.h"
#include "pid.h"
#include "controller.h"
#include "timer.h"
#include "flash.h"
#include "string.h"
#include <stdlib.h>
#include <stdio.h>

uint16_t BlueToothBufIndex ;
uint32_t BlueToothNumbers ; 
char BlueTooth_data_to_send[100];                            //发送数据缓存
char USARTxBlueTooth_RX_DATA[USARTxBlueTooth_RX_LEN];        //接收数据缓存
dt_flag_t Bluetooth;					                        //需要发送数据的标志 
BluetoothParse BluetoothParseMsg = {2,2,0,0};

char BlueRecvData ;
uint8_t BlueRcvLen ; 
/*
 * 函数名：BluetoothSendByte
 * 描述  ：蓝牙单字符发送
 * 输入  ：pbyte发送的字符
 * 返回  ：
 */    
void BluetoothSendByte( char pbyte)      
{
    USART_SendData(USART3, pbyte);         
    while( USART_GetFlagStatus(USART3,USART_FLAG_TC)!= SET);  
}
/*
 * 函数名：BluetoothSendStr
 * 描述  ：蓝牙字符串发送
 * 输入  ：pstr发送的字符串首地址
 * 返回  ：
 */    
void BluetoothSendStr(char *pstr)
{
	unsigned short i,len;
	len = strlen(pstr);
	for(i=0; i<len; i++)
        BluetoothSendByte(*pstr++);
}
/*
 * 函数名：pu8printf
 * 描述  ：串口打印数组
 * 输入  ：name字符串文本名, pbuf待打印数组 , count数组宽度 ，Mode显示模式
 * 返回  ：
 */    
void pu8printf(char* name, uint8_t* pbuf, uint32_t count, uint8_t Mode)
{
	uint32_t index;
	printf("%s:\r\n", name);
	for(index = 0; index < count; index++)
	{
		if(0 == Mode)
		{
			printf("%02d ", pbuf[index]);
		}
		else if(1 == Mode)
		{
			printf("%02X ", pbuf[index]);
		}
        else if(2 == Mode)
        {
//            printf("%s ", pbuf[index]);
        }
	}
	printf("\r\n");
}
/*
 * 函数名：BluetoothUsart_init
 * 描述  ：蓝牙串口初始化
 * 输入  ：波特率
 * 返回  ：
 */    
void BluetoothUsart_init(u32 bound)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;

	RCC_APB2PeriphClockCmd(BLUETOOTH_RCC | RCC_APB2Periph_AFIO, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);

    //  TX
	GPIO_InitStructure.GPIO_Pin = BLUETOOTH_TX_PIN;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(BLUETOOTH_PORT, &GPIO_InitStructure);
    //  RX
	GPIO_InitStructure.GPIO_Pin = BLUETOOTH_RX_PIN;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(BLUETOOTH_PORT, &GPIO_InitStructure);

	USART_InitStructure.USART_BaudRate = bound; 
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;                     //8bits
	USART_InitStructure.USART_StopBits = USART_StopBits_1;                          //stop bit is 1
	USART_InitStructure.USART_Parity = USART_Parity_No;                             //no parity
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //no Hardware Flow Control
	USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;                 //enable tx and rx
	USART_Init(BLUETOOTH_USARTx, &USART_InitStructure);

    USART_ITConfig(BLUETOOTH_USARTx,USART_IT_RXNE,ENABLE);                          //rx interrupt is enable
	USART_Cmd(BLUETOOTH_USARTx, ENABLE);    
}  
 

/*
 * 函数名：USART3_IRQHandler
 * 描述  ：蓝牙串口接收中断处理函数
 * 输入  ：
 * 返回  ：
 */    
void USART3_IRQHandler(void)                                 
{     
    if(USART_GetITStatus(BLUETOOTH_USARTx, USART_IT_RXNE) != RESET)  
    {  
        if(BlueToothBufIndex > USARTxBlueTooth_RX_LEN)
            BlueToothBufIndex = 0;
		USARTxBlueTooth_RX_DATA[BlueToothBufIndex++] = USART_ReceiveData(BLUETOOTH_USARTx);
  
        BlueToothNumbers ++;
        USART_ClearITPendingBit(BLUETOOTH_USARTx, USART_IT_RXNE);                             //清除空闲中断标志
    }      
}   
/*
 * 函数名：ParseBluetoothMessage
 * 描述  ：解析蓝牙app发送过来的数据包
 * 输入  ：pInput收到的蓝牙遥控数据首地址 , blueParseMsg解析后保存的蓝牙数据有效信息
 * 返回  ：err
 */
/*
 * 协议形式：字符串
 * 帧头：# 帧尾：*
 * 例如：#9,1,1,2,2,1*  
 * 每位代表含义：9：数据长度(固定) 1：模式 1：关闭/开始状态 2：蓝牙摇杆X轴数据 2：蓝牙摇杆Y轴数据 1：校验值(累加和减去7)
*/
uint8_t ParseBluetoothMessage(char *pInput , BluetoothParse *blueParseMsg)
{
    unsigned char plen,sum,check;
    ErrorStatus err;
    char *pdata = pInput;
    
    while(( pdata-pInput ) < BlueToothBufIndex )
    {
        if(*pdata == '#')//  #9,1,1,2,2,1*                                                      //接收到数据的包头#
        {
            plen = (unsigned char)atof(strtok(pdata+1, ","));                                   //解析长度length
            if(plen == 9)
            {
                /* 将读出的数据进行累加校验 */ 
                sum =   (unsigned char)int_abs( ((int)atof(strtok(pdata+3, ",")) + 
                        (int)atof(strtok(NULL, ",")) +
                        (int)atof(strtok(NULL, ",")) + 
                        (int)atof(strtok(NULL, ","))) - 7 );
                /* 读出累加校验数据 */
                check = (unsigned char)atof(strtok(NULL, ",")) ;                                //累加校验数据
                if(sum == check)                                                                //校验匹配成功才进行数据解析     
                {
                    blueParseMsg->ModeRocker = (unsigned char)atof(strtok(pdata+3, ","));       //模式数据
                    blueParseMsg->OpenCloseStatus = (unsigned char)atof(strtok(pdata+5, ","));  //关闭/开始状态数据
                    blueParseMsg->Xrocker = (unsigned char)atof(strtok(pdata+7, ","));          //摇杆X数据
                    blueParseMsg->Yrocker = (unsigned char)atof(strtok(pdata+9, ","));          //摇杆Y数据
                    err = SUCCESS;
                }
                else
                {
                    err = ERROR;  
                }
            }
            else 
            {
                err = ERROR;
            }
        }
        else 
        {
            err = ERROR;
        }
        pdata++;
    }
    BlueToothBufIndex = 0;

    return err ;
}

mpu6050：

cs 复制代码 
#include "mpu6050.h"
#include "iic.h"
#include "systick.h"
#include "filter.h"
#include "flash.h"
#include <string.h>
#include "imath.h"
#include "led.h"

S16_XYZ  acc_raw = {0};                  //加速度计原始数据存储
S16_XYZ  gyro_raw = {0};                 //陀螺仪原始数据存储
SI_F_XYZ  gyro_raw_cal = {0};                 //陀螺仪用于校准的原始数据存储
SI_F_XYZ acc_raw_f = {0};
SI_F_XYZ gyro_raw_f = {0};
SI_F_XYZ acc_att_lpf = {0};
SI_F_XYZ gyro_lpf = {0};
SI_F_XYZ gyro_offset = {0,0,0} ;         //陀螺仪零偏数据存储
_Mpu6050_data Mpu = {0};

_GYRO_CAL CalGyro = {0};                 //陀螺仪校准相关变量存储
/*
 * 函数名：mpu6050_init
 * 描述  ：初始化MOU6050配置
 * 输入  ：    
 * 返回  ： 
 */
void mpu6050_init(void)
{
	IIC_Write_One_Byte(0xD0,PWR_MGMT_1, 0x80);		
	tdelay_ms(100);													
	IIC_Write_One_Byte(0xD0,PWR_MGMT_1, 0x00);              //唤醒mpu		
 
    /* when DLPF is disabled( DLPF_CFG=0 or 7),陀螺仪输出频率 = 8kHz; 
       when DLPFis enabled,陀螺仪输出频率 = 1KHz 
       fs(采样频率) = 陀螺仪输出频率 / (1 + SMPLRT_DIV)*/	
	IIC_Write_One_Byte(0xD0,SMPLRT_DIV, 0x00);		        //sample rate.  Fsample= 1Khz/(<this value>+1) = 1000Hz	
	IIC_Write_One_Byte(0xD0,MPU_CONFIG, 0x03);              //内部低通  acc:44hz	gyro:42hz
	IIC_Write_One_Byte(0xD0,GYRO_CONFIG, 0x18);			    // gyro scale  ：+-2000°/s
	IIC_Write_One_Byte(0xD0,ACCEL_CONFIG, 0x10);			// Accel scale ：+-8g (65536/16=4096 LSB/g)    			   			
}
/*
 * 函数名：get_data
 * 描述  ：两字节数据读取并合成一个16位数据
 * 输入  ：REG_Address寄存器地址    
 * 返回  ：合成的16位数据 
 */
static int get_data(unsigned char REG_Address)
{
	unsigned char H,L;
	H = IIC_Read_One_Byte(0xD0,REG_Address);
	L = IIC_Read_One_Byte(0xD0,REG_Address+1);
	return ((H<<8)+L);   
}
/*
 * 函数名：get_mpu_id
 * 描述  ：读取mpu6050的ID
 * 输入  ：     
 * 返回  ：芯片ID 
 */
uint8_t get_mpu_id(void)
{
    u8 mpu_id;
    mpu_id = IIC_Read_One_Byte(0xD0,WHO_AM_I);
    return mpu_id;
}
/*
 * 函数名：get_acc_raw
 * 描述  ：读取加速度计三轴原始数据
 * 输入  ：     
 * 返回  ：     
 */
void get_acc_raw(void)
{
    acc_raw.x = get_data(ACCEL_XOUT_H);
    acc_raw.y = get_data(ACCEL_YOUT_H);
    acc_raw.z = get_data(ACCEL_ZOUT_H); 

	acc_raw_f.x = (float)acc_raw.x;
	acc_raw_f.y = (float)acc_raw.y;
	acc_raw_f.z = (float)acc_raw.z;
}
/*
 * 变量名：gyro_30hz_parameter
 * 描述  ：巴特沃斯低通滤波器参数
 * 输入  ：     
 * 返回  ：     
 */
_Butterworth_parameter gyro_30hz_parameter =
{
    //200hz---30hz
    1,  -0.7477891782585,    0.272214937925,
    0.1311064399166,   0.2622128798333,   0.1311064399166 
}; 

_Butterworth_data   gyro_butter_data[3];
/*
 * 函数名：get_gyro_raw
 * 描述  ：读取陀螺仪三轴原始数据 & 进行校准 & 低通滤波
 * 输入  ：     
 * 返回  ：     
 */
void get_gyro_raw(void)
{
    gyro_raw.x = get_data(GYRO_XOUT_H) - gyro_offset.x;
    gyro_raw.y = get_data(GYRO_YOUT_H) - gyro_offset.y;
    gyro_raw.z = get_data(GYRO_ZOUT_H) - gyro_offset.z;        
    
    gyro_raw_f.x = (float)butterworth_lpf(((float)gyro_raw.x),&gyro_butter_data[0],&gyro_30hz_parameter);
    gyro_raw_f.y = (float)butterworth_lpf(((float)gyro_raw.y),&gyro_butter_data[1],&gyro_30hz_parameter);
    gyro_raw_f.z = (float)butterworth_lpf(((float)gyro_raw.z),&gyro_butter_data[2],&gyro_30hz_parameter);
    
    gyro_raw_cal.x = (float)get_data(GYRO_XOUT_H);
    gyro_raw_cal.y = (float)get_data(GYRO_YOUT_H);
    gyro_raw_cal.z = (float)get_data(GYRO_ZOUT_H);
}
/*
 * 函数名：get_iir_factor
 * 描述  ：求取IIR滤波器的滤波因子
 * 输入  ：out_factor滤波因子首地址，Time任务执行周期，Cut_Off滤波截止频率     
 * 返回  ：     
 */
void get_iir_factor(float *out_factor,float Time, float Cut_Off)
{
	*out_factor = Time /( Time + 1/(2.0f * PI * Cut_Off) );
}
//IIR低通滤波器(加速度)
void acc_iir_lpf(SI_F_XYZ *acc_in,SI_F_XYZ *acc_out,float lpf_factor)
{
	acc_out->x = acc_out->x + lpf_factor*(acc_in->x - acc_out->x); 
	acc_out->y = acc_out->y + lpf_factor*(acc_in->y - acc_out->y); 
	acc_out->z = acc_out->z + lpf_factor*(acc_in->z - acc_out->z); 
}
//加速度计滤波参数  
_Butterworth_parameter acc_5hz_parameter =
{
  //200hz---1hz
//  1,   -1.955578240315,   0.9565436765112,
//  0.000241359049042, 0.000482718098084, 0.000241359049042
  //200hz---2hz
//  1,   -1.911197067426,   0.9149758348014,
//  0.0009446918438402,  0.00188938368768,0.0009446918438402
    //200hz---5hz
    1,                  -1.778631777825,    0.8008026466657,
    0.005542717210281,   0.01108543442056,  0.005542717210281
    //200hz---10hz
//    1,   -1.561018075801,   0.6413515380576,
//    0.02008336556421,  0.04016673112842,  0.02008336556421
    //200hz---15hz
//    1,   -1.348967745253,   0.5139818942197,
//    0.04125353724172,  0.08250707448344,  0.04125353724172
    //200hz---20hz
//    1,    -1.14298050254,   0.4128015980962,
//    0.06745527388907,   0.1349105477781,  0.06745527388907
    //200hz---30hz
//    1,  -0.7477891782585,    0.272214937925,
//    0.1311064399166,   0.2622128798333,   0.1311064399166 
}; 

_Butterworth_data   acc_butter_data[3];
/*
 * 函数名：acc_butterworth_lpf
 * 描述  ：巴特沃斯加速度低通滤波
 * 输入  ：acc_in滤波前的加速度首地址，acc_out滤波后的输出加速度数据首地址     
 * 返回  ：     
 */
void acc_butterworth_lpf(SI_F_XYZ *acc_in,SI_F_XYZ *acc_out)
{
    acc_out->x = butterworth_lpf(acc_in->x,&acc_butter_data[0],&acc_5hz_parameter);
    acc_out->y = butterworth_lpf(acc_in->y,&acc_butter_data[1],&acc_5hz_parameter);
    acc_out->z = butterworth_lpf(acc_in->z,&acc_butter_data[2],&acc_5hz_parameter);    
}
/*
 * 函数名：get_acc_g
 * 描述  ：原始加速度转为重力加速度g为单位数据
 * 输入  ：acc_in原始的加速度首地址，acc_out以g为单位的加速度数据首地址     
 * 返回  ：     
 */
void get_acc_g(SI_F_XYZ *acc_in,SI_F_XYZ *acc_out)
{
	acc_out->x = (float)(acc_in->x * acc_raw_to_g);
	acc_out->y = (float)(acc_in->y * acc_raw_to_g);
	acc_out->z = (float)(acc_in->z * acc_raw_to_g);
}
/*
 * 函数名：get_rad_s
 * 描述  ：原始陀螺仪数据转为弧度/秒为单位的数据
 * 输入  ：gyro_in原始的陀螺仪数据首地址，gyro_out以rad/s为单位的陀螺仪数据首地址     
 * 返回  ：     
 */
void get_rad_s(SI_F_XYZ *gyro_in,SI_F_XYZ *gyro_out)
{
	gyro_out->x = (float)(gyro_in->x * gyro_raw_to_radian_s);
	gyro_out->y = (float)(gyro_in->y * gyro_raw_to_radian_s);
	gyro_out->z = (float)(gyro_in->z * gyro_raw_to_radian_s);
}
/*
 * 函数名：get_deg_s
 * 描述  ：原始陀螺仪数据转为度/秒为单位的数据
 * 输入  ：gyro_in原始的陀螺仪数据首地址，gyro_deg_out以deg/s为单位的陀螺仪数据首地址     
 * 返回  ：     
 */
void get_deg_s(SI_F_XYZ *gyro_in,SI_F_XYZ *gyro_deg_out)
{
	gyro_deg_out->x = (float)(gyro_in->x * gyro_raw_to_deg_s);
	gyro_deg_out->y = (float)(gyro_in->y * gyro_raw_to_deg_s);
	gyro_deg_out->z = (float)(gyro_in->z * gyro_raw_to_deg_s);    
}

/*
 * 函数名：gyro_cal
 * 描述  ：陀螺仪零偏数据校准
 * 输入  ：gyro_in原始的静止时陀螺仪数据首地址    
 * 返回  ：     
 */
void gyro_cal(SI_F_XYZ *gyro_in)
{
    if(CalGyro.flag==1 && 1)                                    
    {
        if(CalGyro.i < GyroCalSumValue)		                                        //原始静止数据500次累加求取平均	
        {                       
            CalGyro.offset.x += gyro_in->x; 
            CalGyro.offset.y += gyro_in->y;
            CalGyro.offset.z += gyro_in->z;
            CalGyro.i++;
        }
        else
        {
            CalGyro.i = 0;
            CalGyro.OffsetFlashWrite.x = CalGyro.offset.x / GyroCalSumValue;        //得到三轴的零偏 
            CalGyro.OffsetFlashWrite.y = CalGyro.offset.y / GyroCalSumValue;        //得到三轴的零偏
            CalGyro.OffsetFlashWrite.z = CalGyro.offset.z / GyroCalSumValue;        //得到三轴的零偏

            /* 将陀螺仪零偏写入flash */
            FlashWriteNineFloat(SENSOR_CAL_ADDRESS, 0,0,0,
                                                    0,0,0,
                                                    CalGyro.OffsetFlashWrite.x,CalGyro.OffsetFlashWrite.y,CalGyro.OffsetFlashWrite.z);  
            /* 校准完数据之后立马读取出来进行使用 */
            flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS,  &CalGyro.None.x,
                                                                        &CalGyro.None.y,
                                                                        &CalGyro.None.z, 
                                                                        &CalGyro.None.x,    
                                                                        &CalGyro.None.y,
                                                                        &CalGyro.None.z,

                                                                        &CalGyro.OffsetFlashRead.x,
                                                                        &CalGyro.OffsetFlashRead.y,
                                                                        &CalGyro.OffsetFlashRead.z);
            /* 判断是否正确读出 */
            if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0)
            {
                _set_val(&gyro_offset,&CalGyro.OffsetFlashRead);                    //陀螺仪零偏设置
                CalGyro.success = 1;                                                //校准成功
                _led.sta = 0;
            }
            else 
            {
                CalGyro.success = 2;                                                //校准失败
            }
            CalGyro.offset.x = 0;
            CalGyro.offset.y = 0;
            CalGyro.offset.z = 0;
            CalGyro.flag = 0;                                                       //校准标志位清除
        }
    }
}

/*
 * 函数名：ReadCalData
 * 描述  ：陀螺仪零偏校准数据进行读取
 * 输入  ：      
 * 返回  ：err     
 */
int ReadCalData(void)
{
    ErrorStatus err;
    flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS,  &CalGyro.None.x,
                                                                &CalGyro.None.y,
                                                                &CalGyro.None.z, 
                                                                &CalGyro.None.x,    
                                                                &CalGyro.None.y,
                                                                &CalGyro.None.z,

                                                                &CalGyro.OffsetFlashRead.x,
                                                                &CalGyro.OffsetFlashRead.y,
                                                                &CalGyro.OffsetFlashRead.z);
    /* 判断是否正确读出 */
    if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0)
    {
        _set_val(&gyro_offset, &CalGyro.OffsetFlashRead);                           //陀螺仪零偏设置
        err = SUCCESS ;
    }
    else
    {
        err = ERROR;
    }
        
    return err;
}
相关推荐
TangDuoduo000540 分钟前
【Cortex-M4分支跳转指令、内存访问指令、ARM AAPCS规则、异常处理】
stm32
石头明月1 小时前
Altium Designer背钻（Back Drilling）设置
嵌入式硬件
影阴1 小时前
stm32 定时器 + hal实现滴答定时器控制led闪烁
stm32·单片机·嵌入式硬件
Darken031 小时前
基于 STM32 ——GPIO输出
单片机·学习·gpio·硬件
阿容1234561 小时前
stm32平衡小车- 03
stm32·单片机·嵌入式硬件
LXY_BUAA2 小时前
《嵌入式操作系统》_从uboot官方移植_02_20251126
linux·单片机·嵌入式硬件
学习路上_write2 小时前
FREERTOS_定时器——创建和基本使用
c语言·开发语言·c++·stm32·嵌入式硬件
黑客思维者2 小时前
凌科芯安LKT6850安全MCU的技术特性与多领域应用
单片机·嵌入式硬件·安全·硬件加密
达不溜的日记2 小时前
BootLoader—基于CAN的FBL详解
网络·stm32·嵌入式硬件·mcu·车载系统·软件工程·信息与通信
热门推荐
01GitHub 镜像站点02UV安装并设置国内源03安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）04BongoCat - 跨平台键盘猫动画工具05本地部署阿里最新开源的Z-Image06Linux下V2Ray安装配置指南07Meta第三代“分割一切”模型——SAM 3本地部署教程：首支持文本提示分割，400万概念、30毫秒响应，检测分割追踪一网打尽08【保姆级教程】免费使用Gemini3的5种方法！免翻墙/国内直连09Labelme从安装到标注：零基础完整指南1046个Nano-banana 精选提示词，持续更新中