1、说明
悟空穿越机原始使用飞控板载的气压计进行定高,实际飞行过程中,由于气压计的自身的飘移,飞机无法非常稳定的定在一定的高度;为了解决这一问题,所以需要更高稳定性的高度传感器;这里我选用的是北醒的tof,TFmini -s;
2、硬件连接
(1)北醒tof连接示意图
可以看到,使用该tof使用串口进行通信;
(2)wukong飞控连接示意图
此处我将tof连接到来飞控的uart5上;
(3)硬件连接示意图
3、软件驱动
(1)tof.c
#include <stdbool.h>
#include <string.h>
#include "config.h"
#include "tof.h"
#include "atkp.h"
#include "config_param.h"
#include "usblink.h"
/*FreeRTOS相关头文件*/
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "usart.h"
static enum
{
waitForStartByte1,
waitForStartByte2,
waitForDisLow,
waitForDisHig,
waitForStrLow,
waitForStrHig,
waitForTempLow,
waitForTempHig,
waitForSum,
}rxState;
static xQueueHandle tofDataQueue;
static tof_t tof_date;
static bool tof_present_flag =false;
bool tof_init(void)
{
tofDataQueue = xQueueCreate(1, sizeof(tof_t));
return tof_present_flag;
}
bool tof_present(void)
{
return tof_present_flag;
}
//USB虚拟串口接收ATKPPacket任务
void TofRxTask(void *param)
{
u8 c;
u8 dataIndex = 0;
u8 cksum = 0;
rxState = waitForStartByte1;
tof_init();//静态队列的声明 要在本地
while(1)
{
if (uart5GetDataWithTimout(&c))
{
switch(rxState)
{
case waitForStartByte1:
rxState = (c == 0x59) ? waitForStartByte2 : waitForStartByte1;
cksum = c;
break;
case waitForStartByte2:
rxState = (c == 0x59) ? waitForDisLow : waitForStartByte1;
cksum += c;
break;
case waitForDisLow:
tof_date.distance = c;
cksum += c;
rxState = waitForDisHig;
break;
case waitForDisHig:
tof_date.distance += c<<8;
cksum += c;
rxState = waitForStrLow;
break;
case waitForStrLow:
tof_date.strength = c;
cksum += c;
rxState = waitForStrHig;
break;
case waitForStrHig:
tof_date.strength += c<<8;
cksum += c;
rxState = waitForTempLow;
break;
case waitForTempLow:
tof_date.temp = c;
cksum += c;
rxState = waitForTempHig;
break;
case waitForTempHig:
tof_date.temp += c<<8;
cksum += c;
rxState = waitForSum;
break;
case waitForSum:
if(cksum == c)
{
#if TOF_PRINTF
u8 _cnt=0;
atkp_t p;
sprintf(p.data,"%d,%d\r\n",tof_date.distance,tof_date.strength);
for(uint8_t i = 0 ;i<30;i++)
{
if(p.data[i] == 0x0D)
{
if(p.data[++i] == 0x0A)
{
p.dataLen = ++i;
break;
}
}
}
usblinkSendPacket(&p);
#endif
tof_date.asl = (float)tof_date.distance;
xQueueOverwrite(tofDataQueue, &tof_date);
tof_present_flag = true;
}
rxState = waitForStartByte1;
break;
default:
ASSERT(0);
break;
}
}
else /*超时处理*/
{
rxState = waitForStartByte1;
}
}
}
bool tofUpdate(tof_t *tof)
{
return (pdTRUE == xQueueReceive(tofDataQueue, tof, 0));
}
(2)tof.h
#ifndef __TOF_H
#define __TOF_H
#include <stdbool.h>
#include "atkp.h"
#include "stabilizer_types.h"
void TofRxTask(void *param);
bool tofUpdate(tof_t *tof);
bool tof_present(void);
#endif
(3)usart.c
#include "sys.h"
#include "usart.h"
//
//如果使用ucos,则包括下面的头文件即可.
#if SYSTEM_SUPPORT_OS
#include "FreeRTOS.h" //FreeRTOS 使用
#include "task.h"
#include "semphr.h"
#include "queue.h"
#endif
//
//本程序只供学习使用,未经作者许可,不得用于其它任何用途
//ALIENTEK STM32F4探索者开发板
//串口1初始化
//正点原子@ALIENTEK
//技术论坛:www.openedv.com
//修改日期:2014/6/10
//版本:V1.5
//版权所有,盗版必究。
//Copyright(C) 广州市星翼电子科技有限公司 2009-2019
//All rights reserved
//********************************************************************************
//V1.3修改说明
//支持适应不同频率下的串口波特率设置.
//加入了对printf的支持
//增加了串口接收命令功能.
//修正了printf第一个字符丢失的bug
//V1.4修改说明
//1,修改串口初始化IO的bug
//2,修改了USART_RX_STA,使得串口最大接收字节数为2的14次方
//3,增加了USART_REC_LEN,用于定义串口最大允许接收的字节数(不大于2的14次方)
//4,修改了EN_USART1_RX的使能方式
//V1.5修改说明
//1,增加了对UCOSII的支持
//
//
//加入以下代码,支持printf函数,而不需要选择use MicroLIB
#if 1
#pragma import(__use_no_semihosting)
//标准库需要的支持函数
struct __FILE
{
int handle;
};
FILE __stdout;
//定义_sys_exit()以避免使用半主机模式
void _sys_exit(int x)
{
x = x;
}
extern int consolePutchar(int ch);
//重定义fputc函数
int fputc(int ch, FILE *f)
{
while((USART1->SR&0X40)==0);//循环发送,直到发送完毕
USART1->DR = (u8) ch;
// consolePutchar(ch); /*打印到上位机*/
return ch;
}
#endif
void uart1_send_str(uint8_t* data,uint8_t length)
{
for(int i = 0 ; i<length;i++)
{
while((USART1->SR&0X40)==0);//循环发送,直到发送完毕
USART1->DR = (u8)data[i];
}
}
void uart5_send_str(uint8_t* data,uint8_t length)
{
for(int i = 0 ; i<length;i++)
{
while((UART5->SR&0X40)==0);//循环发送,直到发送完毕
UART5->DR = (u8)data[i];
}
}
#if EN_USART1_RX //如果使能了接收
//串口1中断服务程序
//注意,读取USARTx->SR能避免莫名其妙的错误
u8 USART_RX_BUF[USART_REC_LEN]; //接收缓冲,最大USART_REC_LEN个字节.
//接收状态
//bit15, 接收完成标志
//bit14, 接收到0x0d
//bit13~0, 接收到的有效字节数目
u16 USART_RX_STA=0; //接收状态标记
static xQueueHandle usbDataDelivery;
static xQueueHandle uart5DataDelivery;
//初始化IO 串口1
//bound:波特率
void uart_init(u32 bound)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
//串口1对应引脚复用映射
GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
//USART1端口配置
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度50MHz
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9,PA10
//USART1 初始化设置
USART_InitStructure.USART_BaudRate = bound;//波特率设置
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(USART1, &USART_InitStructure); //初始化串口1
USART_Cmd(USART1, ENABLE); //使能串口1
USART_ClearFlag(USART1, USART_FLAG_TC);
#if EN_USART1_RX
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
//Usart1 NVIC 配置
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=15;//抢占优先级15
NVIC_InitStructure.NVIC_IRQChannelSubPriority =0; //子优先级0
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器、
#endif
usbDataDelivery = xQueueCreate(128, sizeof(uint8_t)); /*队列 128个消息*/
}
void uart5_init(u32 bound)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD,ENABLE); //使能GPIOD时钟
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC,ENABLE); //使能GPIOC时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5,ENABLE);//使能UART5时钟
//串口1对应引脚复用映射
GPIO_PinAFConfig(GPIOC,GPIO_PinSource12,GPIO_AF_UART5); //GPIOC12复用为UART5
GPIO_PinAFConfig(GPIOD,GPIO_PinSource2,GPIO_AF_UART5); //GPIOD2复用为UART5
//USART1端口配置
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; //GPIOA9与GPIOA10
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度50MHz
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOC,&GPIO_InitStructure); //初始化PC12
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 ; //GPIOA9与GPIOA10
GPIO_Init(GPIOD,&GPIO_InitStructure); //初始化PD2
//USART1 初始化设置
USART_InitStructure.USART_BaudRate = bound;//波特率设置
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(UART5, &USART_InitStructure); //初始化串口1
USART_Cmd(UART5, ENABLE); //使能串口1
USART_ClearFlag(UART5, USART_FLAG_TC);
#if EN_USART1_RX
USART_ITConfig(UART5, USART_IT_RXNE, ENABLE);//开启相关中断
//Usart1 NVIC 配置
NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;//串口1中断通道
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=15;//抢占优先级15
NVIC_InitStructure.NVIC_IRQChannelSubPriority =0; //子优先级0
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器、
#endif
uart5DataDelivery = xQueueCreate(128, sizeof(uint8_t)); /*队列 128个消息*/
}
void USART1_IRQHandler(void) //串口1中断服务程序
{
u8 Res;
portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收中断(接收到的数据必须是0x0d 0x0a结尾)
{
Res =USART_ReceiveData(USART1);//(USART1->DR); //读取接收到的数据
xQueueSendFromISR(usbDataDelivery, &Res, &xHigherPriorityTaskWoken);
}
}
bool usart1GetDataWithTimout(uint8_t *c)
{
if (xQueueReceive(usbDataDelivery, c, 1000) == pdTRUE) /*接收usbDataDelivery(1024个容量)消息*/
{
return true;
}
*c = 0;
return false;
}
void UART5_IRQHandler(void) //串口1中断服务程序
{
u8 Res;
portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
if(USART_GetITStatus(UART5, USART_IT_RXNE) != RESET) //接收中断(接收到的数据必须是0x0d 0x0a结尾)
{
Res =USART_ReceiveData(UART5);//(USART1->DR); //读取接收到的数据
xQueueSendFromISR(uart5DataDelivery, &Res, &xHigherPriorityTaskWoken);
}
}
bool uart5GetDataWithTimout(uint8_t *c)
{
if (xQueueReceive(uart5DataDelivery, c, 1000) == pdTRUE) /*接收usbDataDelivery(1024个容量)消息*/
{
return true;
}
*c = 0;
return false;
}
#endif
上面的code主要模仿串口1的中断处理,当串口收到1个字节的时候,就将字节存入一个消息队列中;且提供给外部一个读取字节的函数接口;该函数接口在tofTask中被调用,如果飞控收到tof发过来的一个字节,那就一个字节一个字节进行解析;
4、注意的点
(1)uart时钟初始化
在开发串口驱动的时候,发现串口无法正常的收发数据,在反复确认引脚没有接错之后,还是无法正常工作,最后发现由于串口5是uart,不是usart,即时钟挂在AHB1下面;所以没有成功初始化成功;
(2)静态消息队列本地初始化
在将tof的最终数据填入到tof的消息队列时,只要运行xQueueOverwrite(tofDataQueue, &tof_date);就会抛异常,最后发现是一开始的队列初始化没有在本地执行导致的;