一个航模佬手搓固定翼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;
typedef struct {
//加速度原始数据
int16_t Accel_X_RAW;
int16_t Accel_Y_RAW;
int16_t Accel_Z_RAW;
//陀螺仪原始数据
int16_t Gyro_X_RAW;
int16_t Gyro_Y_RAW;
int16_t Gyro_Z_RAW;
}MPU6050_Data;
void MPU_I2C_Write(unsigned char dev_addr,unsigned char mem_addr , unsigned char data);
void MPU_I2C_Read(unsigned char dev_addr,unsigned char mem_addr , unsigned char *buf,unsigned char len);
void MPU_Set_LPF(uint16_t lpf);
void Set_Mpu_Rate(uint16_t rate);
void MPU_6050_Init(void);
void MPU_Read_Gyro(void);
void MPU_Read_Accel(void);
void MPU_Read_ALL(MPU6050_Data *data);
void MPU6050_calibrate(void);
#endif
mpu6050.c:
#include "mpu6050.h"
unsigned char I2C_Accel_Buf[6];
unsigned char I2C_Gyro_Buf[6];
//加速度计
signed short ax;
signed short ay;
signed short az;
//陀螺仪
signed short gx;
signed short gy;
signed short gz;
//校准基值
signed short ax_l = 0;
signed short ay_l = 0;
signed short az_l = 0;
signed short gx_l = 0;
signed short gy_l = 0;
signed short gz_l = 0;
//注:这里传入的是&hi2c1,既i2c1接口,对于移植和更改读取逻辑来说,仅需要修改MPU_I2C_Write()和MPU_I2C_Read()两个函数的i2c驱动方式(如HAL库,固件库,寄存器,软件模拟等)即可
//I2C写入
void MPU_I2C_Write(unsigned char dev_addr, unsigned char mem_addr, unsigned char data) {
HAL_I2C_Mem_Write(&hi2c1, dev_addr, mem_addr, I2C_MEMADD_SIZE_8BIT, &data, 1, 2);
}
//I2C读出
void MPU_I2C_Read(unsigned char dev_addr, unsigned char mem_addr, unsigned char *buf, unsigned char len) {
HAL_I2C_Mem_Read(&hi2c1, dev_addr, mem_addr, I2C_MEMADD_SIZE_8BIT, buf, len, 2);
}
//初始化MPU6050
void MPU_6050_Init(void) {
unsigned char temp = 0;
reset_MPU6050:
MPU_I2C_Write(MPU6050_Write_Addr, PowerMem_Register1_Addr, Set_PowerMen_Reset); //MPU复位
HAL_Delay(50);
MPU_I2C_Write(MPU6050_Write_Addr, PowerMem_Register1_Addr, Set_PowerMen_Start_XPPL); //MPU唤醒 使用X轴作为时钟源
MPU_I2C_Read(MPU6050_Write_Addr, MPU_Id_Addr, &temp,
1); //读器件ID
if (temp != 0x68) {
goto reset_MPU6050;
}
MPU_I2C_Write(MPU6050_Write_Addr, MPU_Gyro_Addr,
(Set_Gyro_Range << 3)); //陀螺仪量程范围设置 2000
MPU_I2C_Write(MPU6050_Write_Addr, MPU_Accel_Addr, (Set_Accel_Range << 3)); //加速计量程范围设置 ±2G
Set_Mpu_Rate(
50); //采样率分频
MPU_I2C_Write(MPU6050_Write_Addr, MPU_Int_Pin_Addr, Set_Int_Enable); //关闭所有中断
MPU_I2C_Write(MPU6050_Write_Addr, MPU_User_Addr,
Set_User_I2c); //关闭I2C主模式
MPU_I2C_Write(MPU6050_Write_Addr, MPU_Fifo_En_Addr, Set_Fifo_Enable); //关闭FIFO
MPU_I2C_Write(MPU6050_Write_Addr, MPU_Int_Pin_Addr, Set_Int_Pin_L); //INT低电平有效
MPU_I2C_Write(MPU6050_Write_Addr, PowerMem_Register2_Addr, Sat_Start_Mpu); //MPU开始工作
}
//读取陀螺仪的数据
void MPU_Read_Gyro(void) {
MPU_I2C_Read(MPU6050_Write_Addr, GYRO_Xout_H, I2C_Gyro_Buf, 6);
gx = (I2C_Gyro_Buf[0] << 8) + I2C_Gyro_Buf[1] - gx_l;
gy = (I2C_Gyro_Buf[2] << 8) + I2C_Gyro_Buf[3] - gy_l;
gz = (I2C_Gyro_Buf[4] << 8) + I2C_Gyro_Buf[5] - gz_l;
}
//读取加速度计的数据
void MPU_Read_Accel(void) {
MPU_I2C_Read(MPU6050_Write_Addr, ACCEL_Xout_H, I2C_Accel_Buf, 6);
ax = (I2C_Accel_Buf[0] << 8) + I2C_Accel_Buf[1] - ax_l;
ay = (I2C_Accel_Buf[2] << 8) + I2C_Accel_Buf[3] - ay_l;
az = (I2C_Accel_Buf[4] << 8) + I2C_Accel_Buf[5] - az_l;
}
/*
* Read_Mpu_ALL(MPU6050_Data *data)
* 一次性读取全部数值
* 参数:MPU6050_Data类型指针,指向包含mpu6050数据包的变量
* 返回值:空
*/
void MPU_Read_ALL(MPU6050_Data *data) {
MPU_Read_Gyro();
MPU_Read_Accel();
data->Gyro_X_RAW = gx;
data->Gyro_Y_RAW = gy;
data->Gyro_Z_RAW = gz;
data->Accel_X_RAW = ax;
data->Accel_Y_RAW = ay;
data->Accel_Z_RAW = az;
}
//设置采样频率
//返回值:0,设置成功
// 其他,设置失败
void Set_Mpu_Rate(uint16_t rata) {
uint8_t data;
if (rata > 1000)
rata = 1000;
if (rata < 4)
rata = 4;
data = 1000 / rata - 1;
MPU_I2C_Write(MPU6050_Write_Addr, MPU_Out_Fre_DIV_Addr, data); //采样率分频
MPU_Set_LPF(rata / 2); //自动设置LPF为采样率的一半
}
//设置MPU6050的数字低通滤波器
//返回值:0,设置成功
// 其他,设置失败
void MPU_Set_LPF(uint16_t lpf) {
uint8_t data = 0;
if (lpf >= 188)data = 1;
else if (lpf >= 98)data = 2;
else if (lpf >= 42)data = 3;
else if (lpf >= 20)data = 4;
else if (lpf >= 10)data = 5;
else data = 6;
MPU_I2C_Write(MPU6050_Write_Addr, MPU_Dlpf_Addr, data);
}
/*自校准模式
MPU6050校准函数
将IMU水平放置,z轴向上时,启动校准
思路是计算CL_cnt个周期的平均值,得到校准参数
*/
#define CL_cnt 128
void MPU6050_calibrate(void) {
unsigned short i;
signed int temp[6] = {0};
for (i = 0; i < CL_cnt; i++) {
HAL_Delay(10);
MPU_Read_Gyro();
MPU_Read_Accel();
temp[0] += ax;
temp[1] += ay;
temp[2] += az;
temp[3] += gx;
temp[4] += gy;
temp[5] += gz;
}
ax_l = temp[0] / CL_cnt;
ay_l = temp[1] / CL_cnt;
az_l = temp[2] / CL_cnt - (0xffff >> 2); //平放时z轴有重力加速度g,减去g值
gx_l = temp[3] / CL_cnt;
gy_l = temp[4] / CL_cnt;
gz_l = temp[5] / CL_cnt;
}
如果想要修改iic读写方式(hal库,标准库,软件iic),仅需修改MPU_I2C_Write()和MPU_I2C_Read()两个函数即可
使用时,先进行陀螺仪初始化和校准,声明一个MPU6050_Data类型的结构体,并将其取地址调用MPU_Read_ALL()函数即可
使用示例:
main.c:
#include "mpu6050.h"
int main(void)
{
MPU_6050_Init();//MPU6050初始化
MPU6050_calibrate();//MPU6050校准
MPU6050_Data mpu_data;
while(1)
{
MPU_Read_ALL(&mpu_data);
usb_printf("ax:%d ay:%d az:%d gx:%d gy:%d gz:%d\r\n",
mpu_data.Accel_X_RAW,
mpu_data.Accel_Y_RAW,
mpu_data.Accel_Z_RAW,
mpu_data.Gyro_X_RAW,
mpu_data.Gyro_Y_RAW,
mpu_data.Gyro_Z_RAW);
HAL_Delay(1000);
}
}
磁力计(hmc5883l)硬件iic原始数据的获取
hmc5883l.h:
#ifndef __HMC5883L_H__
#define __HMC5883L_H__
#include "i2c.h"
#define HMC5883L_Addr 0x3C //hmc5883l地址
typedef struct {
int16_t Magnetic_X_RAW;
int16_t Magnetic_Y_RAW;
int16_t Magnetic_Z_RAW;
}HMC_Data;
//注:对于移植和更改读取逻辑来说,仅需要修改HMC_I2C_Write()和HMC_I2C_Read()两个函数的i2c驱动方式(如HAL库,固件库,寄存器,软件模拟等)即可
void HMC_I2C_Write(unsigned char dev_addr,unsigned char mem_addr , unsigned char data);
void HMC_I2C_Read(unsigned char dev_addr,unsigned char mem_addr , unsigned char *buf,unsigned char len);
void Single_Write_HMC5883(unsigned char REG_Address, unsigned char REG_data);
unsigned char Single_Read_HMC5883(unsigned char REG_Address);
void HMC_Init(void);
void HMC_Read_ALL(HMC_Data *data);
#endif
hmc5883l.c:
#include "hmc5883l.h"
unsigned char HMC_BUF[8];//数据存储
//注:对于移植和更改读取逻辑来说,仅需要修改HMC_I2C_Write()和HMC_I2C_Read()两个函数的i2c驱动方式(如HAL库,固件库,寄存器,软件模拟等)即可
//i2c写
void HMC_I2C_Write(unsigned char dev_addr, unsigned char mem_addr, unsigned char data) {
HAL_I2C_Mem_Write(&hi2c2, dev_addr, mem_addr, I2C_MEMADD_SIZE_8BIT, &data, 1, 2);
}
//i2c读
void HMC_I2C_Read(unsigned char dev_addr, unsigned char mem_addr, unsigned char *HMC_BUF, unsigned char len) {
HAL_I2C_Mem_Read(&hi2c2, dev_addr, mem_addr, I2C_MEMADD_SIZE_8BIT, HMC_BUF, len, 2);
}
void Single_Write_HMC5883(unsigned char REG_Address, unsigned char REG_data) {
HMC_I2C_Write(HMC5883L_Addr, REG_Address, REG_data);
}
unsigned char Single_Read_HMC5883(unsigned char REG_Address) {
unsigned char REG_data;
HMC_I2C_Read(HMC5883L_Addr, REG_Address, ®_data, 1);
return REG_data;
}
void HMC_Init(void) {
Single_Write_HMC5883(0x00,
(3 << 5) |
(1 << 4) |
(0)
);
Single_Write_HMC5883(0x01,
(1 << 5) |
(0));
Single_Write_HMC5883(0x02,0x00);
}
void HMC_Read_ALL(HMC_Data *data){
HMC_I2C_Read(HMC5883L_Addr, 0x03, HMC_BUF, 6);//读取数据存放在HMC_BUF中
data->Magnetic_X_RAW = (short)(HMC_BUF[0] << 8 | HMC_BUF[1]);//X方向磁感应强度
data->Magnetic_Z_RAW = (short)(HMC_BUF[2] << 8 | HMC_BUF[3]);//Z方向磁感应强度
data->Magnetic_Y_RAW = (short)(HMC_BUF[4] << 8 | HMC_BUF[5]);//Y方向磁感应强度
}
与mpu6050一样,只需要修改iic的读取和写入两个函数的实现,就可以移植该库
使用时,先进行磁力计初始化,声明一个HMC_Data类型的结构体,并将其取地址调用HMC_Read_ALL()函数即可,磁力计因为不同地区磁场方向不一样,干扰不一样,且磁力计的校准需要较高的算力,所以校准部分通过电脑上位机实现,此处暂时略过
使用示例:
main.c:
#include "hmc5883l.h"
int main(void)
{
HMC_Init();//HMC5883l初始化
HMC_Data hmc_data;
while(1){
HMC_Read_ALL(&hmc_data);
usb_printf("X:%d Y:%d Z:%d\r\n",
hmc_data.Magnetic_X_RAW,
hmc_data.Magnetic_Y_RAW,
hmc_data.Magnetic_Z_RAW);
HAL_Delay(1000);
}
}
实验现象:
注:关于cubemx生成代码iic中的bug
在i2c.c中,初始化函数这里void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle)
里面的__HAL_RCC_I2C1_CLK_ENABLE();使能i2c1的时钟,需要放在GPIO初始化之前,否则可能iic没有输出,i2c2同理:
void HAL_I2C_MspInit(I2C_HandleTypeDef *i2cHandle) {
GPIO_InitTypeDef GPIO_InitStruct = {0};
if (i2cHandle->Instance == I2C1) {
/* USER CODE BEGIN I2C1_MspInit 0 */
__HAL_RCC_I2C1_CLK_ENABLE();//就是这里
/* USER CODE END I2C1_MspInit 0 */
__HAL_RCC_GPIOB_CLK_ENABLE();
/**I2C1 GPIO Configuration
PB6 ------> I2C1_SCL
PB7 ------> I2C1_SDA
*/
GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* I2C1 clock enable */
__HAL_RCC_I2C1_CLK_ENABLE();
/* USER CODE BEGIN I2C1_MspInit 1 */
/* USER CODE END I2C1_MspInit 1 */
} else if (i2cHandle->Instance == I2C2) {
/* USER CODE BEGIN I2C2_MspInit 0 */
__HAL_RCC_I2C2_CLK_ENABLE();//还有这里
/* USER CODE END I2C2_MspInit 0 */
__HAL_RCC_GPIOB_CLK_ENABLE();
/**I2C2 GPIO Configuration
PB10 ------> I2C2_SCL
PB11 ------> I2C2_SDA
*/
GPIO_InitStruct.Pin = GPIO_PIN_10 | GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* I2C2 clock enable */
__HAL_RCC_I2C2_CLK_ENABLE();
/* USER CODE BEGIN I2C2_MspInit 1 */
/* USER CODE END I2C2_MspInit 1 */
}
}
PPM输出
1.输出原理
一端PPM信号周期为20ms,由三部分信号组成:
- 起始:500μs的低电平,用于表示信号的开始
- 中间部分:用500-1500μs的高电平表示每个通道的值,值的范围在0-1000,可以理解为先500μs的高电平表示信号开始,而后的1-1000高电平持续表示当前通道值,该通道输出结束后用500μs的低电平表示该通道结束,而后再输出高电平表示下个通道输出的开始,以此循环往复直到通道输出结束
- 最后:用高电平填充完本周期直至20ms结束
那么如何实现呢?
为了不影响主程序的运行,我们可以选择使用定时器去控制PPM输出IO高低电平转换的时间间隔,每到定时器触发中断时,由中断函数进入ppm输出函数并进行io高低电平的转换,而后重置定时器装载值为所需时间,如此一来就可以实现后台持续输出ppm信号。
2.代码实现
在前文中,我们已经通过cubemx配置好了PPM输出的引脚以及所需的定时器并开启了定时器全局中断使能,所以当定时器触发时,会进入中断函数,中断会调用这个函数:
__weak void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
当我们启用的计时器TIM2触发中断时,此处传入的参数是&htim2,可以看到,改函数为虚函数,我们可以在main.c中重写该函数从而自定义:
main.c:
//TIM2中断,用于进行ppm信号的产生
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
if (htim->Instance == TIM2) {//判断是几号定时器
//进入到ppm输出
...
}
}
下面是ppm实现的库(此处定义为8通道输出,实际可以最多9通道,按需修改,注释详细描述了整个程序运作过程):
ppm.h:
#ifndef __PPM_H__
#define __PPM_H__
#include "tim.h"
//定时器需要先进行使能
//使用ppm库时需要在这里配置好输出引脚高低电平的操作,且初始状态ppm引脚要求为低电平
#define PPM_Out_H HAL_GPIO_WritePin(PPM_GPIO_Port, PPM_Pin, GPIO_PIN_SET);
#define PPM_Out_L HAL_GPIO_WritePin(PPM_GPIO_Port, PPM_Pin, GPIO_PIN_RESET);
void PPM_Out(const uint16_t ppm_data[]);
#endif
ppm.c:
#include "ppm.h"
uint8_t ppm_count = 1;//步骤次数
uint16_t total_time = 0;//总时间
void PPM_Out(const uint16_t ppm_data[]/*ppm各通道值,要求值在1~1000之间*/){
uint32_t time;//时长缓存
__HAL_TIM_CLEAR_IT(&htim2, TIM_IT_UPDATE);//清除更新中断标识
//ppm_count从1开始,用于计数,当小于17时,说明处于数据输出阶段
if(ppm_count < 17){
switch (ppm_count % 2) {
case 0://当ppm_count为偶数时,表示通道关闭
PPM_Out_L
time = 499;//以0.5ms低电平结束
total_time += time;
__HAL_TIM_SET_AUTORELOAD(&htim2, time);//设置tim2的自动重载值
break;
case 1:// 当ppm_count为奇数时,表示通道的开启
PPM_Out_H
time = ppm_data[ppm_count / 2] + 499;//获取当前通道值(范围在1-1000),PPM要求时间在500-1500us,故加上499
total_time += time;
__HAL_TIM_SET_AUTORELOAD(&htim2, time);//设置tim2的自动重载值
break;
}
ppm_count++;//更新计数
} else{//如果大于等于17,说明输出完毕
switch (ppm_count) {
case 17://当等于17时,20ms剩下的时间输出高电平
PPM_Out_H
time = 19500 - total_time;
__HAL_TIM_SET_AUTORELOAD(&htim2, time);
ppm_count++;
break;
case 18://当等于18时,表示输出完毕,重新开始
PPM_Out_L
__HAL_TIM_SET_AUTORELOAD(&htim2,499);
total_time = 0;
ppm_count = 1;
break;
default:
break;
}
}
}
HAL库可直接使用,仅更换定时器仅需要更换&htim2这部分,若要迁移到固件库,需要修改__HAL_TIM_CLEAR_IT(),__HAL_TIM_SET_AUTORELOAD这两个函数,更改输出引脚仅需要修改PPM_Out_L和PPM_Out_H这两个宏定义
在使用时,需要先执行:
__HAL_TIM_CLEAR_IT(&htim2, TIM_IT_UPDATE);//清除定时器初始化过程中的更新中断标志,避免定时器一启动就中断
HAL_TIM_Base_Start_IT(&htim2);//ppm输出所需定时器tim2中断使能
且需要在之前所说的中断函数中调用ppm的输出函数
示例程序:
main.c:
#include "ppm.h"
uint16_t ppm_data[8];//ppm各通道值
//TIM2中断重写,用于进行ppm信号的产生
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
if (htim->Instance == TIM2) {
//进入到ppm输出函数
PPM_Out(ppm_data);
}
}
int main(void){
//随意赋值用于测试
ppm_data[0] = 100;
ppm_data[1] = 200;
ppm_data[2] = 300;
ppm_data[3] = 400;
ppm_data[4] = 500;
ppm_data[5] = 600;
ppm_data[6] = 700;
ppm_data[7] = 800;
//ppm输出启动
__HAL_TIM_CLEAR_IT(&htim2, TIM_IT_UPDATE);//清除定时器初始化过程中的更新中断标志,避免定时器一启动就中断
HAL_TIM_Base_Start_IT(&htim2);//ppm输出所需定时器tim2中断使能
while(1){
}
}
3.实际测试
因为手里没有音频插头用来连接遥控器,所以我们使用arduino开发板对ppm信号输出进行检测
在网上随便找一段输入程序:
PPMTest.cpp:
#include <Arduino.h>
#define channumber 8 //How many channels have your radio?
int value[channumber];
void setup()
{
Serial.begin(57600); //Serial Begin
pinMode(3, INPUT); //Pin 3 as input
}
void loop()
{
while(pulseIn(3, LOW) < 5000){} //Wait for the beginning of the frame
for(int x=0; x<=channumber-1; x++)//Loop to store all the channel position
{
value[x]=pulseIn(3, LOW);
}
for(int x=0; x<=channumber-1; x++)//Loop to print and clear all the channel readings
{
Serial.print(value[x] - 500); //Print the value
Serial.print(" ");
value[x]=0; //Clear the value afeter is printed
}
Serial.println(""); //Start a new line
}
烧录到arduino,将arduino的3号引脚连接到stm32的PPM引脚(PB0),GND接起来,两者都上电,在波特率中选择57600
结果:
姿态解算-MadgwickAHRS
这里尚未实现,敬请等待后续。。。。