一个航模佬手搓固定翼fpv头追的底层驱动和姿态算法研究笔记·一 部分驱动
序
自航模入九州,人皆以目飞为主,而今fpv穿越具之兴,爱固翼fpv者亦增,为得实感,工研寻首之器,然其价之高,其能之差,其yaw者亦有偏移,众皆苦之。是以余与友欲研新,解yaw者之移,破价高之局,润航模之市,从者乃硬者底之驱及姿解之法也。
项目介绍
本项目使用stm32f103CBT6作为主控,基于HAL库进行开发(博主是菜鸟不会用固件库😂),使用cubemx管理和配置项目代码,利用clion + openocd的方式进行开发(参考稚晖君的文章),本文介绍包含一下几项:
- cubemx配置:STM32f103CBT6
- usb cdc(vcp):USB虚拟串口,用于debug以及后续调试
- mpu6050:六轴陀螺仪加速度计的驱动以及校准,用于获取原始数据
- HMC5883L:三轴磁力计的驱动,用于获取原始数据
- PPM:用于将通道值输出给遥控器
- Madgwick:9轴数据滤波融合,姿态解算
项目配置(cubemx)
- 芯片选择STM32F103CBT6
- Debug模式选择SW,开启外部高速时钟
- 打开I2C1(用于mpu6050)和I2C1(用于hmc5883l),其中I2C1启用FastMode,时钟频率为400000Hz
- 打开usb,并在中间层设备中配置USB_DEVICE并将Class For FS IP改为CDC(VCP)
- 任意启用一个没有使用到的gpio用于ppm输出(此处使用PB0),初始值设置为Low,模式设置为推挽输出,不进行上拉或者下拉,输出速率改为HIGH,标志为PPM(可自定义)
- 启用一个空闲的通用定时器(此处使用tim2)用于ppm输出,时钟源(Clock Source)设置为内部时钟,预分频器(Prescaler)设置为71(该芯片主频为72MHZ,预分频值+1 = 72,分频后可以得到周期为1μs的定时周期),计数方向为UP,自动装载值(AutoReload Register)设置为499,不进行时钟分频(No Division) ,自动装载的使能(auto-preload)设置为Enable,在NVIC Settings中开启TIM2的中断
- 时钟树:主频配置为72Mhz,usb频率为48Mhz
- 项目管理:当使用clion进行开发的时候,在cubemx中将项目名字更改为和clion项目名称相同,路径相同,工具链选择STM32CubeIDE,选择将.c和.h文件分开存储,之后便可以启动项目了
usb cdc(vcp)配置
在项目配置中,cubemx已经帮我们生成了usb_device的底层驱动以及应用层程序,所以我们只需要进行简单的修改便能应用到我们的项目中:
我们需要修改的是位于USB_DEVICE/App/下的usbd_cdc_if.h和usbd_cdc_if.c
可以看到,cubemx已经帮我们生成好了接收和发送的程序,我们在这里新定义一个函数方便进行虚拟串口的输出
(注:用户代码需要写到cubemx注释的BEGIN和END之间,否则在cubemx更新代码的时候会删掉你所写的代码)
- 我们在usbd_cdc_if.h中声明一个函数:
void usb_printf(const char *format, ...);
- 然后在usbd_cdc_if.c中实现它
#include <stdarg.h>
void usb_printf(const char *format, ...)
{
va_list args;
uint32_t length;
va_start(args, format);
length = vsnprintf((char *)UserTxBufferFS, APP_TX_DATA_SIZE, (char *)format, args);
va_end(args);
CDC_Transmit_FS(UserTxBufferFS, length);
}
这样我们只需要在main.c中引入usb_cdc_if.h,便可以使用usb_printf()进行usb串口的输出字符串(有兴趣的可以通过重定向stdio库的printf()函数达到同样的效果),就像这样:
main.c:
#include "usbd_cdc_if.h"
int i = 1;
int main(void)
{
usb_printf("Head Begin ");
usb_printf("i = %d\r\n",i);
}
加速度计&陀螺仪(mpu6050)硬件iic读取原始数据和校准
关于mpu6050原始数据的获取,往上教程其实很多了,基本查一下就能得到
mpu6050.h:
#ifndef __MPU6050_H__
#define __MPU6050_H__
#include "i2c.h"
#define MPU6050_Write_Addr 0xD0 //默认状态 地址为0X69 只有六位数据 使用IC7位寻址 ,第七位根据MPU6050引脚AD0在判断 AD0=0 第七位0 AD0=1 第七位1 第八位I2C读1 写0 我这里使用AD0=0低电平
#define MPU6050_Read_Addr 0xD1
#define MPU_Out_Fre_DIV_Addr 0x19 //采样频率地址 默认8Khz 如果开了低通滤波器 则变为1Khz
#define MPU_Dlpf_Addr 0x1A //低通滤波器地址
#define MPU_Gyro_Addr 0x1B //陀螺仪数据地址 第八七六位 XYZ轴自检给1 不自检给0 第五四两位 用于给陀螺仪量程范围 00-0~250 01-0~500 10-0~1000 11-0~2000 其余位0
#define MPU_Accel_Addr 0x1C //加速计数据地址 第八七六位 XYZ轴自检给1 不自检给0 第五四两位 用于给陀螺仪量程范围 00-±2G 01-±4G 10-±8G 11-±16G 其余位0
#define MPU_Id_Addr 0x75 //MPU ID的地址
#define MPU_Int_Addr 0X38 //mpu中断使能地址
#define MPU_User_Addr 0x6A //MPU用户控制地址 用于控制FIFO I2C主模式
#define MPU_Fifo_En_Addr 0x23 //MPU的FIFO使能地址
#define MPU_Int_Pin_Addr 0x37 //INT中断地址
#define PowerMem_Register1_Addr 0x6B //电源管理1的地址
#define PowerMem_Register2_Addr 0x6C //电源管理2的地址
#define Set_PowerMen_Reset 0x80 //在电源管理 输入 这个数据 为MPU 复位
#define Set_PowerMen_Start_XPPL 0x01 //在电源管理 输入 这个数据 为MPU唤醒 且使用X轴的时钟源
#define Set_Smplrt_Div 0x00 //采样分频
#define Set_Dlpf_Div 0x00 //低通滤波器模式
#define Set_Gyro_Range 0x11 //设置陀螺仪的量程范围
#define Set_Accel_Range 0x00 //设置加速计的量程范围
#define Set_Int_Enable 0x00 //设置关闭全部中断
#define Set_User_I2c 0x00 //禁止FIFO I2C主模式
#define Set_Fifo_Enable 0x00 //禁止FIFO模式
#define Set_Int_Pin_L 0x80 //INT脚低电平有效
#define Sat_Start_Mpu 0x00 //设置启动MPU陀螺仪
//加速度计 数据寄存器地址
#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 GYRO_Xout_H 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
//六轴数据
extern signed short ax;
extern signed short ay;
extern signed short az;
extern signed short gx;
extern signed short gy;
extern signed short gz;
//六轴校准基值
extern signed short ax_l;
extern signed short ay_l;
extern signed short az_l;
extern signed short gx_l;
extern signed short gy_l;
extern signed short gz_l;