【recode】tof定高(一)——添加tof驱动

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);就会抛异常,最后发现是一开始的队列初始化没有在本地执行导致的;

  • 0
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值