接上篇看看飞控的姿态结算
在第一篇中飞控初始化了systicks时钟后，并没有做任何处理这里不仅仅是计时，也作为姿态解算的时间标志，
在它的中断函数里有：
/*
* 函数名：SysTick_Handler
* 描述 ：滴答中断函数
* 输入 ：无
* 输出 ：无
*/
void SysTick_Handler(void)
{
static u16 cnt = 0;
static u8 ms = 0;
cnt++;
Prepare_Data(); //准备数据
if(Is_WFLY_Unlock())//判断是否解锁与是否在执行解锁动作
{
Is_WFLY_Lock(); //判断是否在执行上锁动作
IMU_Update(); //姿态更新频率为1KHz
ms++;
if(ms == 2) //控制频率500Hz
{
ms = 0;
WFLY_PWM_Convert();
Control(Angle,Exp_Angle);
}
if(cnt % 500 == 0)
{
LED_LEFT_BACK_TOGGLE;
LED_RIGHT_BACK_TOGGLE;
cnt = 0;
}
}
else//未解锁
{
Motor_PWM_Update(0,0,0,0);//确保安全
if(cnt % 1000 == 0)
{
LED_LEFT_BACK_TOGGLE;
LED_RIGHT_BACK_TOGGLE;
cnt = 0;
}
}
}首先是姿态数据收集
即函数Prepare_Data(); //准备数据
/*
* 函数名：Prepare_Data
* 描述 ：读取MPU6500原始数据；加速度计预滤波
* 输入 ：Now_Angle：当前姿态；Exp_Angle：期望姿态
* 输出 ：无
*/
void Prepare_Data(void)
{
MPU6500_ReadValue(); //读取MPU6500原始值
Acc_Flitter(); //加速度计预滤波
}/*
* 函数名：MPU6500_ReadValue
* 描述 ：读取MPU6500原始数据
* 输入 ：无
* 输出 ：无
*/
void MPU6500_ReadValue(void)
{
uint8_t i;
MPU6500_CS(0); //使能SPI传输
SPI1_Read_Write_Byte(ACCEL_XOUT_H|0x80); //从加速度计的寄存器开始进行读取陀螺仪和加速度计的值//发送读命令+寄存器号
for(i = 0;i < 14;i++) //一共读取14字节的数据
{
mpu6500_buf[i] = SPI1_Read_Write_Byte(0xff); //输入0xff,因为slave不识别
}
if(offset_flag == 0)
{
MPU6500_Acc.X = BYTE16(s16, mpu6500_buf[0], mpu6500_buf[1]) - MPU6500_Acc_Offset.X;
MPU6500_Acc.Y = BYTE16(s16, mpu6500_buf[2], mpu6500_buf[3]) - MPU6500_Acc_Offset.Y;
MPU6500_Acc.Z = BYTE16(s16, mpu6500_buf[4], mpu6500_buf[5]);
MPU6500_Gyro.X = BYTE16(s16, mpu6500_buf[8], mpu6500_buf[9]) - MPU6500_Gyro_Offset.X;
MPU6500_Gyro.Y = BYTE16(s16, mpu6500_buf[10], mpu6500_buf[11]) - MPU6500_Gyro_Offset.Y;
MPU6500_Gyro.Z = BYTE16(s16, mpu6500_buf[12], mpu6500_buf[13]) - MPU6500_Gyro_Offset.Z;
mpu6500_tempreature_temp = BYTE16(s16, mpu6500_buf[6], mpu6500_buf[7]);
mpu6500_tempreature = (float)(35000+((521+mpu6500_tempreature_temp)*100)/34); // 原来分母为340，现在分子*100，即：扩大1000倍；
mpu6500_tempreature = mpu6500_tempreature/1000;
if(( -4 < MPU6500_Gyro.X ) && (MPU6500_Gyro.X < 4) ) MPU6500_Gyro.X = 0;
if(( -4 < MPU6500_Gyro.Y ) && (MPU6500_Gyro.Y < 4) ) MPU6500_Gyro.Y = 0;
if(( -4 < MPU6500_Gyro.Z ) && (MPU6500_Gyro.Z < 4) ) MPU6500_Gyro.Z = 0;
}
else if(offset_flag) //MPU6500处于校准模式
{
MPU6500_Acc.X = BYTE16(s16, mpu6500_buf[0], mpu6500_buf[1]);
MPU6500_Acc.Y = BYTE16(s16, mpu6500_buf[2], mpu6500_buf[3]);
MPU6500_Acc.Z = BYTE16(s16, mpu6500_buf[4], mpu6500_buf[5]);
MPU6500_Gyro.X = BYTE16(s16, mpu6500_buf[8], mpu6500_buf[9]);
MPU6500_Gyro.Y = BYTE16(s16, mpu6500_buf[10], mpu6500_buf[11]);
MPU6500_Gyro.Z = BYTE16(s16, mpu6500_buf[12], mpu6500_buf[13]);
}
MPU6500_CS(1); //禁止SPI传输
}/*
* 函数名：MPU6500_Init
* 描述 ：MPU6500初始化函数
* 输入 ：无
* 输出 ：0：初始化失败 1：初始化成功
*/
u8 MPU6500_Init(void)
{
SPI1_Init(); //MPU6500 IO口和SPI初始化
if(MPU6500_Read_Reg(WHO_AM_I) == 0x70) //正确读取到6500的地址
{
MPU6500_Write_Reg(PWR_MGMT_1,0X80); //电源管理,复位MPU6500
Delay_Ms(100);
MPU6500_Write_Reg(SIGNAL_PATH_RESET,0X07);//陀螺仪、加速度计、温度计复位
Delay_Ms(100);
MPU6500_Write_Reg(PWR_MGMT_1,0X01); //选择时钟源
MPU6500_Write_Reg(PWR_MGMT_2,0X00); //使能加速度计和陀螺仪
MPU6500_Write_Reg(CONFIG,0X02); //低通滤波器 0x02 92hz (3.9ms delay) fs=1khz
MPU6500_Write_Reg(SMPLRT_DIV,0X00); //采样率1000/(1+0)=1000HZ
MPU6500_Write_Reg(GYRO_CONFIG,0X18); //陀螺仪测量范围 0X18 正负2000度
MPU6500_Write_Reg(ACCEL_CONFIG,0x10); //加速度计测量范围 0X00 正负8g
MPU6500_Write_Reg(ACCEL_CONFIG2,0x00); //加速度计速率1khz 滤波器460hz (1.94ms delay)
return 1;
}
else return 0;
}
现在进入SPI1_Init
/*
* 函数名：SPI1_Init
* 描述 ：SPI1初始化
* 输入 ：无
* 输出 ：无
*/
void SPI1_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
SPI_InitTypeDef SPI_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB,ENABLE);//GPIOA时钟使能
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1,ENABLE ); //SPI1时钟使能
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; //MPU6500片选信号
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
MPU6500_CS(1); //MPU6500片选取消
GPIO_SetBits(GPIOA,GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7); //PB13/14/15上拉
SPI_Cmd(SPI1,DISABLE); //SPI1失能
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; //SPI设置为双线双向全双工
SPI_InitStructure.SPI_Mode = SPI_Mode_Master; //SPI主机
SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;//发送接收8位帧结构
SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;//时钟悬空低
SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;//数据捕获于第1个时钟沿
SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;//NSS信号由软件控制
SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;//定义波特率预分频的值:波特率预分频值为16（72M/8=9M）
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;//数据传输从MSB位开始
SPI_InitStructure.SPI_CRCPolynomial = 7; //CRC值计算的多项式
SPI_Init(SPI1, &SPI_InitStructure); //根据SPI_InitStruct中指定的参数初始化外设SPIx寄存器
SPI_Cmd(SPI1,ENABLE);//使能SPI1
SPI1_Read_Write_Byte(0xff);//启动传输
} 这里引入了MPU6500的片选，MPU6500_CS(1);
定义为
#define MPU6500_CS(X) (X==0)?GPIO_ResetBits(GPIOB,GPIO_Pin_0):GPIO_SetBits(GPIOB,GPIO_Pin_0) //MPU6500片选信号另外有一个函数SPI1_Read_Write_Byte
/*
* 函数名：SPI1_Read_Write_Byte
* 描述 ：读写一个字节
* 输入 ：TxData:要写入的字节
* 输出 ：读取到的字节
*/
u8 SPI1_Read_Write_Byte(uint8_t TxData)
{
u8 retry = 0;
while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET) //检查指定的SPI标志位设置与否:发送缓存空标志位
{
retry++;
if(retry > 250) return 0;
}
SPI_I2S_SendData(SPI1, TxData); //通过外设SPIx发送一个数据
retry = 0;
while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE) == RESET) //检查指定的SPI标志位设置与否:接受缓存非空标志位
{
retry++;
if(retry > 250) return 0;
}
return SPI_I2S_ReceiveData(SPI1); //返回通过SPIx最近接收的数据
}另外有两个函数要注意
/*
* 函数名：MPU6500_Read_Reg
* 描述 ：SPI读取寄存器
* 输入 ：reg:指定的寄存器地址
* 输出 ：reg_val：reg寄存器地址对应的值
*/
u8 MPU6500_Read_Reg(uint8_t reg)
{
uint8_t reg_val;
MPU6500_CS(0); //使能SPI传输
SPI1_Read_Write_Byte(reg | 0x80); //发送读命令+寄存器号
reg_val = SPI1_Read_Write_Byte(0xff); //读取寄存器值
MPU6500_CS(1); //禁止MPU9250
return(reg_val);
}另外一个哈数
* 函数名：MPU6500_Write_Reg
* 描述 ：SPI写入寄存器
* 输入 ：reg:指定的寄存器地址；value：写入的值
* 输出 ：status：返回状态值
*/
u8 MPU6500_Write_Reg(uint8_t reg,uint8_t value)
{
uint8_t status;
MPU6500_CS(0); //使能SPI传输
status = SPI1_Read_Write_Byte(reg); //发送写命令+寄存器号
SPI1_Read_Write_Byte(value); //写入寄存器值
MPU6500_CS(1); //禁止MPU9500
return(status); //返回状态值
}现在开始读取数据
首先是有系统时钟的中断函数进入Prepare_Data(); //准备数据
定义：
/*
* 函数名：Prepare_Data
* 描述 ：读取MPU6500原始数据；加速度计预滤波
* 输入 ：Now_Angle：当前姿态；Exp_Angle：期望姿态
* 输出 ：无
*/
void Prepare_Data(void)
{
MPU6500_ReadValue(); //读取MPU6500原始值
Acc_Flitter(); //加速度计预滤波
}
/*
* 函数名：MPU6500_ReadValue
* 描述 ：读取MPU6500原始数据
* 输入 ：无
* 输出 ：无
*/
void MPU6500_ReadValue(void)
{
uint8_t i;
MPU6500_CS(0); //使能SPI传输
SPI1_Read_Write_Byte(ACCEL_XOUT_H|0x80); //从加速度计的寄存器开始进行读取陀螺仪和加速度计的值//发送读命令+寄存器号
for(i=0;i<14;i++) //一共读取14字节的数据
{
mpu6500_buf[i] =SPI1_Read_Write_Byte(0xff);//输入0xff,因为slave不识别
}
if(offset_flag == 0)
{
MPU6500_Acc.X = BYTE16(s16, mpu6500_buf[0], mpu6500_buf[1]) - MPU6500_Acc_Offset.X;
MPU6500_Acc.Y = BYTE16(s16, mpu6500_buf[2], mpu6500_buf[3]) - MPU6500_Acc_Offset.Y;
MPU6500_Acc.Z = BYTE16(s16, mpu6500_buf[4], mpu6500_buf[5]);
MPU6500_Gyro.X = BYTE16(s16, mpu6500_buf[8], mpu6500_buf[9]) - MPU6500_Gyro_Offset.X;
MPU6500_Gyro.Y = BYTE16(s16, mpu6500_buf[10], mpu6500_buf[11]) - MPU6500_Gyro_Offset.Y;
MPU6500_Gyro.Z = BYTE16(s16, mpu6500_buf[12], mpu6500_buf[13]) - MPU6500_Gyro_Offset.Z;
mpu6500_tempreature_temp=BYTE16(s16, mpu6500_buf[6], mpu6500_buf[7]);
mpu6500_tempreature=(float)(35000+((521+mpu6500_tempreature_temp)*100)/34); // 原来分母为340，现在分子*100，即：扩大1000倍；
mpu6500_tempreature = mpu6500_tempreature/1000;
if(( -4 < MPU6500_Gyro.X ) && (MPU6500_Gyro.X < 4) ) MPU6500_Gyro.X = 0;
if(( -4 < MPU6500_Gyro.Y ) && (MPU6500_Gyro.Y < 4) ) MPU6500_Gyro.Y = 0;
if(( -4 < MPU6500_Gyro.Z ) && (MPU6500_Gyro.Z < 4) ) MPU6500_Gyro.Z = 0;
}
else if(offset_flag) //MPU6500处于校准模式
{
MPU6500_Acc.X = BYTE16(s16, mpu6500_buf[0], mpu6500_buf[1]);
MPU6500_Acc.Y = BYTE16(s16, mpu6500_buf[2], mpu6500_buf[3]);
MPU6500_Acc.Z = BYTE16(s16, mpu6500_buf[4], mpu6500_buf[5]);
MPU6500_Gyro.X = BYTE16(s16, mpu6500_buf[8], mpu6500_buf[9]);
MPU6500_Gyro.Y = BYTE16(s16, mpu6500_buf[10], mpu6500_buf[11]);
MPU6500_Gyro.Z = BYTE16(s16, mpu6500_buf[12], mpu6500_buf[13]);
}
MPU6500_CS(1); //禁止SPI传输
}然后加速度数据滤波
/*
* 函数名：Acc_Flitter
* 描述 ：加速度计预滤波
* 递推最小二乘估计/滑动滤波
* 1：无需存储全部数据，取得一组观测数据便可以估计一次参数
* 且都能够在一个采样周期内完成，计算量小，占用存储空间小
* 2：具有一定的实时处理能力
* 输入 ：无
* 输出 ：无
*/
void Acc_Flitter()
{
static u8 filter_cnt = 0;
static u8 cnt;
S_INT32_XYZ Temp;
Temp.X = 0;
Temp.Y = 0;
Temp.Z = 0;
Acc_Buf[filter_cnt].X = MPU6500_Acc.X;
Acc_Buf[filter_cnt].Y= MPU6500_Acc.Y;
Acc_Buf[filter_cnt].Z = MPU6500_Acc.Z;
filter_cnt++;
for(cnt =0;cnt <FILTER_LENGTH;cnt++)
{
Temp.X += Acc_Buf[cnt].X;
Temp.Y += Acc_Buf[cnt].Y;
Temp.Z += Acc_Buf[cnt].Z;
}
Acc_Avg.X = Temp.X / FILTER_LENGTH ;//FILTER_LENGTH =20
Acc_Avg.Y = Temp.Y / FILTER_LENGTH;
Acc_Avg.Z = Temp.Z / FILTER_LENGTH;
if(filter_cnt==FILTER_LENGTH) filter_cnt = 0;
}加权平均滤波算法与算数平均滤波算法相类似，算数平均滤波算法相当于加权平均算法中各个信号前所加的权值均为1/N。为了控制N个采样信号中某些采样值的比重，在每个采样点前加入不同的小于1的权值，将占比重大的采样值前乘以大的权值，占比重小的采样值前乘以小的权值，从而实现提高系统对当前所受干扰的灵敏度。将所有乘以权值后的数据相加求平均，得出滤波后的数据，其中，所有的数值前的权值和为1。
SPI，是SerialPeripheral interface的缩写，顾名思义就是串行外围设备接口。是Motorola首先在其MC68HCXX系列处理器上定义的。SPI接口主要应用在EEPROM，FLASH，实时时钟，AD转换器，还有数字信号处理器和数字信号解码器之间。SPI，是一种高速的，全双工，同步的通信总线，并且在芯片的管脚上只占用四根线，节约了芯片的管脚，同时为PCB的布局上节省空间，提供方便，正是出于这种简单易用的特性，现在越来越多的芯片集成了这种通信协议。
SPI通信中可作为从机也可以作为主机，这取决于硬件设计和软件设置。
当器件作为主机时，使用一个IO引脚拉低相应从机的选择引脚(NSS)，传输的起始由主机发送数据来启动，时钟(SCK)信号由主机产生。通过MOSI发送数据，同时通过MISO引脚接收从机发出的数据。
当器件作为从机时，传输在从机选择引脚(NSS)被主机拉低后开始，接收主机输出的时钟信号，在读取主机数据的同时通过MISO引脚输出数据。
通常SPI通过4个引脚与外部器件相连：
●MISO：主设备输入/从设备输出引脚。该引脚在从模式下发送数据，在主模式下接收数据。
●MOSI：主设备输出/从设备输入引脚。该引脚在主模式下发送数据，在从模式下接收数据。
● SCK： 串口时钟，作为主设备的输出，从设备的输入。
● NSS：从设备选择。这是一个可选的引脚，用来选择主/从设备。它的功能是用来作为“片选引脚”,让主设备可以单独地与特定从设备通讯，避免数据线上的冲突。从设备的NSS引脚可以由主设备的一个标准I/O引脚 来驱动。
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
