159 lines
5.5 KiB
C
159 lines
5.5 KiB
C
#include "stm32f4xx.h" // Device header
|
||
#include "stm32f4xx_conf.h"
|
||
#include "Visual.h"
|
||
#include "UART.h"
|
||
#include "gimbal.h"
|
||
#include "AttitudeAlgorithms.h"
|
||
#include"Delay.h"
|
||
|
||
float Visual_Yaw,Visual_Pitch,Visual_Roll,Visual_Delay;//视觉数据偏航角,视觉数据俯仰角
|
||
uint8_t Visual_RxHEXPacket[4],Visual_ReceiveFlag;//视觉数据接收缓冲区,视觉数据接收完成标志位
|
||
|
||
/*
|
||
*函数简介:视觉初始化
|
||
*参数说明:无
|
||
发送至下位机数据
|
||
|数据 1|数据 2|数据 3|数据 4|数据 5|
|
||
|----|----|----|----|----|
|
||
|0x09|0x14|yaw 轴|Pitch 轴|0x18|
|
||
|
||
*返回类型:无
|
||
*备注:初始化UART2(USART1)
|
||
Freq=Sys_APB1TIM/(PSC+1)/(ARR+1)=84MHz/(PSC+1)/(ARR+1)
|
||
*/
|
||
void Visual_Init(void)
|
||
{/*
|
||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE);//开启时钟
|
||
|
||
TIM_InternalClockConfig(TIM12);//选择时基单元的时钟(TIM9)
|
||
|
||
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;//配置时基单元（配置参数）
|
||
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;//配置时钟分频为1分频
|
||
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;//配置计数器模式为向上计数
|
||
TIM_TimeBaseInitStructure.TIM_Period=500-1;//配置自动重装值ARR
|
||
TIM_TimeBaseInitStructure.TIM_Prescaler=840-1;//配置分频值PSC,定时50ms
|
||
TIM_TimeBaseInitStructure.TIM_RepetitionCounter=0;//配置重复计数单元的置为0
|
||
TIM_TimeBaseInit(TIM12,&TIM_TimeBaseInitStructure);//初始化TIM12
|
||
|
||
TIM_ClearFlag(TIM12,TIM_FLAG_Update);//清除配置时基单元产生的中断标志位
|
||
|
||
TIM_ITConfig(TIM12,TIM_IT_Update,ENABLE);//使能更新中断
|
||
|
||
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//选择NVIC分组
|
||
|
||
NVIC_InitTypeDef NVIC_InitStructure1;//配置NVIC（配置参数）
|
||
NVIC_InitStructure1.NVIC_IRQChannel=TIM8_BRK_TIM12_IRQn;//选择中断通道为TIM12
|
||
NVIC_InitStructure1.NVIC_IRQChannelCmd=ENABLE;//使能中断通道
|
||
NVIC_InitStructure1.NVIC_IRQChannelPreemptionPriority=3;//TIM12的抢占优先级
|
||
NVIC_InitStructure1.NVIC_IRQChannelSubPriority=3;//TIM12的响应优先级
|
||
NVIC_Init(&NVIC_InitStructure1);//初始化NVIC
|
||
TIM_Cmd(TIM12,ENABLE);//启动定时器
|
||
*/
|
||
Visual_SendData();
|
||
|
||
}
|
||
void Visual_SendData(void)
|
||
{
|
||
SendData MCUMessage;
|
||
MCUMessage.curr_yaw=AttitudeAlgorithms_DegYaw;
|
||
MCUMessage.curr_pitch=AttitudeAlgorithms_DegPitch;
|
||
MCUMessage.state=(uint8_t)('a');
|
||
MCUMessage.mark=(uint8_t)(0);
|
||
MCUMessage.anti_top=(uint8_t)(0);
|
||
MCUMessage.enemy_color=(uint8_t)(1);
|
||
MCUMessage.delta_x=(uint8_t)(1);
|
||
MCUMessage.delta_y=(uint8_t)(1);
|
||
UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));//UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));
|
||
/*
|
||
UART2_SendByte((float)AttitudeAlgorithms_DegYaw);
|
||
UART2_SendByte((float)AttitudeAlgorithms_DegRoll);
|
||
//UART2_SendByte((float)AttitudeAlgorithms_DegPitch);
|
||
|
||
UART2_SendByte((uint8_t)0);
|
||
UART2_SendByte((uint8_t)'a');
|
||
UART2_SendByte((uint8_t)0);
|
||
|
||
UART2_SendByte((uint8_t)0);
|
||
UART2_SendByte((uint8_t)1);
|
||
UART2_SendByte((uint8_t)1);
|
||
*/
|
||
//UART2_Printf("%f %f a 1 1 1 1 1 ",AttitudeAlgorithms_DegYaw,AttitudeAlgorithms_DegRoll);
|
||
|
||
UART2_SendByte((uint8_t)'\n');
|
||
|
||
|
||
|
||
|
||
//UART2_SendArray((uint8_t *)&MCUMessage, sizeof(MCUMessage));
|
||
}
|
||
/* buff[0] = 's';
|
||
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
|
||
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
|
||
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
|
||
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
|
||
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
|
||
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
|
||
buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
|
||
buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
|
||
buff[9] = 'e';
|
||
*/
|
||
|
||
/*
|
||
*函数简介:UART2串口中断接收视觉数据
|
||
*参数说明:无
|
||
*返回类型:无
|
||
*备注:无
|
||
*/
|
||
void USART1_IRQHandler(void)
|
||
{
|
||
#define DataLength 8//有效数据位数
|
||
char Visual_RxHEXPacket[10];
|
||
|
||
static uint8_t UART2_RxData;
|
||
//char Message[10];
|
||
static int RxHEXState=0;//定义静态变量用于接收模式的选择
|
||
static int pRxHEXState=0;//定义静态变量用于充当计数器
|
||
char startflag;//包头1
|
||
//char endflag;//包头2
|
||
|
||
if(USART_GetITStatus(USART1,USART_IT_RXNE)==SET)//查询接收中断标志位
|
||
{
|
||
UART2_RxData=USART_ReceiveData(USART1);//将数据存入缓存区
|
||
startflag=(char)UART2_RxData;
|
||
|
||
|
||
if(RxHEXState==0)//模式0-等待包头1(0x09)
|
||
{
|
||
|
||
if( startflag =='s')//检测包头
|
||
RxHEXState=1;//转入模式1
|
||
}
|
||
else if(RxHEXState==1)//模式1-接收有效数据
|
||
{
|
||
pRxHEXState=0;//复位计数器
|
||
Visual_RxHEXPacket[pRxHEXState]=UART2_RxData;//接收数据
|
||
pRxHEXState++;
|
||
|
||
if(pRxHEXState>=DataLength)
|
||
RxHEXState=2;//转入模式2
|
||
}
|
||
else if(RxHEXState==2)//模式2-等待包尾
|
||
{
|
||
if(UART2_RxData=='e')//检测包尾
|
||
{
|
||
Visual_Yaw=(float)((uint8_t)Visual_RxHEXPacket[0]<<8 | Visual_RxHEXPacket[1]);//右摇杆右左
|
||
Visual_Pitch=(float)((uint8_t)Visual_RxHEXPacket[2]<<8 | Visual_RxHEXPacket[3]);//右摇杆上下
|
||
Visual_Roll=(float)((uint8_t)Visual_RxHEXPacket[4]<<8 | Visual_RxHEXPacket[5]);//左摇杆右左
|
||
Visual_Delay=(float)((uint8_t)Visual_RxHEXPacket[6]<<8 | Visual_RxHEXPacket[7]);//左摇杆上下
|
||
//Visual_Yaw=(int16_t)((uint16_t)Visual_RxHEXPacket[0]<<8 | Visual_RxHEXPacket[1])*0.01f;
|
||
//Visual_Pitch=(int16_t)((uint16_t)Visual_RxHEXPacket[2]<<8 | Visual_RxHEXPacket[3])*0.01f;
|
||
Visual_ReceiveFlag=1;
|
||
}
|
||
RxHEXState=0;//回到模式0
|
||
}
|
||
}
|
||
|
||
USART_ClearITPendingBit(USART1,USART_IT_RXNE);//清除接收中断标志位
|
||
Visual_SendData();
|
||
}
|