四旋翼无人机光流定位和激光定高2（2中含原代码）
时间：2022-11-09 23:30:01
四旋翼无人机光流定位原代码：
通常，我们购买以下光流模块GL9306
串口输出模块光流数据，格式如下：
串口读取代码如下：
usart.h
#ifndef __USART1_H
#define __USART1_H
#include "stm32f10x.h"
#include
extern void USART1_SendByte(const int8_t *Data,uint8_t len);
extern void USART3_SendByte(const int8_t *Data,uint8_t len);
extern void USART1_Config(void);
extern void USART3_Config(void);
extern int fputc(int ch, FILE *f);
extern void USART1_setBaudRate(uint32_t baudRate);
#endif /* __USART1_H */
usart.c
#include "usart.h"
// 改写光流定位 串口通讯
// 接线方式 光流模块TX - stm32 PA10 RX
void USART1_Config(void)
{
GPIO_InitTypeDef GPIO_InitStructure1;
USART_InitTypeDef USART_InitStructure1;
NVIC_InitTypeDef NVIC_InitStructure1;
/* 配置串口1 （USART1） 时钟*/
/*" 第1步：打开GPIO和USART部件的时钟 "*/
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE );//| RCC_APB2Periph_AFIO, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
/* Configure the NVIC Preemption Priority Bits */
/* 中断串口1 */
NVIC_InitStructure1.NVIC_IRQChannel = USART1_IRQn; //USART1 串口1全局中断
NVIC_InitStructure1.NVIC_IRQChannelPreemptionPriority = 1.//抢占优先级1
NVIC_InitStructure1.NVIC_IRQChannelSubPriority = 1; //子优先级1
/*IRQ通道使能*/
NVIC_InitStructure1.NVIC_IRQChannelCmd = ENABLE;
/*根据NVIC_InitStruct初始化外设中指定的参数NVIC寄存器USART1*/
NVIC_Init(&NVIC_InitStructure1);
/*串口GPIO端口配置*/
/* 配置串口1 （USART1 Tx (PA.09)）*/
GPIO_InitStructure1.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure1.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure1.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure1);
/* 配置串口1 USART1 Rx (PA.10)*/
GPIO_InitStructure1.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure1.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure1);
//USART 初始化设置
USART_InitStructure1.USART_BaudRate =19200./串口波特率
USART_InitStructure1.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
USART_InitStructure1.USART_StopBits = USART_StopBits_1；//一个停止位
USART_InitStructure1.USART_Parity = USART_Parity_No;///无奇偶校准位
USART_InitStructure1.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
USART_InitStructure1.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
USART_Init(USART1, &USART_InitStructure1);
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//打开中断
USART_Cmd(USART1, ENABLE); //使能串口
}
/**************************实现函数**********************************************
*功 能: usart发送一个字节
*********************************************************************************/
void usart1_send(u8 data)
{
USART1->DR = data;
while((USART1->SR&0x40)==0);
}
/**************************************************************************
函数功能：串口1初始化
入口参数：pclk2:PCLK2 时钟频率(Mhz) bound:波特率
返回 值：无
**************************************************************************/
void uart1_init(u32pclk2,u32 bound)
{
float temp;
u16 mantissa;
u16 fraction;
temp=(float)(pclk2*1000000)/(bound*16);//得到USARTDIV
mantissa=temp; //得到整数部分
fraction=(temp-mantissa)*16; //得到小数部分
mantissa<<=4;
mantissa+=fraction;
RCC->APB2ENR|=1<<0; //开启辅助时钟
RCC->APB2ENR|=1<<3; //使能PORTB口时钟
RCC->APB1ENR|=1<<18; //使能串口时钟
GPIOB->CRH&=0XFFFF0FFF;
GPIOB->CRH|=0X00008000;//IO状态设置
GPIOB->ODR|=1<<10;
RCC->APB1RSTR|=1<<18; //复位串口1
RCC->APB1RSTR&=~(1<<18);//停止复位
//波特率设置
USART1->BRR=mantissa; // 波特率设置
USART1->CR1|=0X200C; //1位停止,无校验位.
//使能接收中断
USART1->CR1|=1<<8; //PE中断使能
USART1->CR1|=1<<5; //接收缓冲区非空中断使能
// MY_NVIC_Init(0,0,USART1_IRQn,1);//组2，最低优先级
}
/**************************************************************************
函数功能：串口3接收中断
入口参数：无
返回 值：无
**************************************************************************/
int USART1_IRQHandler(void)
{
static u8 RxBuffer[32];
static u8 _data_cnt = 0;
static u8 state = 0;
u8 data;
u8 sum = 0;
if(USART1->SR&(1<<5))//接收到数据
{
USART1->SR&=~(1<<0);//清除中断标志位
data=USART1->DR;
switch(state)
{
case 0:
if(data==0xFE)
{
state=1;
RxBuffer[_data_cnt++]=data;
}else state = 0;
break;
case 1:
if(data==0x04)
{
state=2;
RxBuffer[_data_cnt++]=data;
}else state = 0;
break;
case 2:
RxBuffer[_data_cnt++]=data;
if(_data_cnt==9)
{
state = 0;
_data_cnt = 0;
sum = (RxBuffer[2] + RxBuffer[3] + RxBuffer[4] + RxBuffer[5]);
if((0xAA == data) && (sum == RxBuffer[6])) //和校验
{
count_temp++;
//读原始数据
Flow_Pitch = ( (s16)(*(RxBuffer+3)<<8)|*(RxBuffer+2) );
Flow_Roll = ( (s16)(*(RxBuffer+5)<<8)|*(RxBuffer+4) );
//累积求位移
Flow_Pitch_i += Flow_Pitch;
Flow_Roll_i += Flow_Roll;
if(count_temp>400)
{
Flag_USART3=1;
X_Flow_Pitch_i+= Flow_Pitch_i/(8200.0f/(this_Final_Distance*1.0f));//this_Final_Distance为实际高度
Y_Flow_Roll_i += Flow_Roll_i /(8200.0f/(this_Final_Distance*1.0f));//this_Final_Distance为实际高度
Flow_Pitch_i=0;//使用完后清零，防止高度提升时位移发生变化
Flow_Roll_i =0;//使用完后清零，防止高度提升时位移发生变化
}
}
}
break;
default:
state = 0;
_data_cnt = 0;
break;
}
}
return 0;
}
//
无人机控制部分代码如下：
//调试时 real_height=400 ,this_Final_Distance暂时赋值为500，即0.5米的高度
real_height=400;
this_Final_Distance=450;
//*************************************姿态角补偿光流*****************************************************//
//光流定位
Roll=Angle.roll; //赋给测量值 即姿态模块取得的实时角度
Pitch=Angle.pitch;
if(Flag_USART3==1) //串口1读到光流数据
{
Flag_USART3 = 0;
Y_change = tan( (Roll -Roll_Y) *angle_to_rad );
X_change = tan( (Pitch-Pitch_X) *angle_to_rad );
Pitch_ang_x += ( X_change- Pitch_ang_x ) *0.3f;//低通滤波 因为光流数据的输出滞后于 实时欧拉角的输出，所以补偿
Roll_ang_y += ( Y_change- Roll_ang_y ) *0.3f;//低通滤波
if(real_height<=800)Mag=0.130f;
else Mag=0.126f;
X_Flow_Pitch_i_final = X_Flow_Pitch_i - (Pitch_ang_x * this_Final_Distance * Mag );
Y_Flow_Roll_i_final = Y_Flow_Roll_i - (Roll_ang_y * this_Final_Distance * Mag );
PK1= X_Flow_Pitch_i_final - last_X_Flow_Pitch_i ;
PK2= Y_Flow_Roll_i_final - last_Y_Flow_Roll_i ;
last_X_Flow_Pitch_i = X_Flow_Pitch_i_final;
last_Y_Flow_Roll_i = Y_Flow_Roll_i_final;
velocity_X = PK1 / 0.005f ;
velocity_Y = PK2 / 0.005f ;
if( velocity_X>+150 ) velocity_X=+150;
if( velocity_Y>+150 ) velocity_Y=+150;
if( velocity_X<-150 ) velocity_X=-150;
if( velocity_Y<-150 ) velocity_Y=-150;
//*********************************摇杆控制-平行Roll\Pitch轴向的位置**************************************//
X_Yaogan=pidPitch.desired /4.0f; //pidPitch.desired为遥杆值
Y_Yaogan=pidRoll.desired/4.0f;
}
//***********************************光流定点位置环******************************************************//
if(X_Yaogan!=0)
{position_X=0;X_POSITION=X_Flow_Pitch_i_final;}
else
position_X = Move_X_position (X_POSITION,X_Flow_Pitch_i_final);
if(Y_Yaogan!=0)
{position_Y=0;Y_POSITION=Y_Flow_Roll_i_final;}
else
position_Y = Move_Y_position (Y_POSITION,Y_Flow_Roll_i_final);
if(position_X>+40)position_X=+40;
if(position_X<-40)position_X=-40;
if(position_Y>+40)position_Y=+40;
if(position_Y<-40)position_Y=-40;
//************************************光流定点速度环*****************************************************//
if(real_height<350)
{
if(X_Yaogan>30)X_Yaogan=30;
if(Y_Yaogan>30)Y_Yaogan=30;
if(X_Yaogan<-30)X_Yaogan=-30;
if(Y_Yaogan<-30)Y_Yaogan=-30;
}
if(real_height<200)
{
X_Yaogan=0;
Y_Yaogan=0;
}
MOVE_X = Move_X_velocity (velocity_X,X_Yaogan-position_X);
MOVE_Y = Move_Y_velocity (velocity_Y,Y_Yaogan-position_Y);
if(MOVE_X>+6) MOVE_X=+6;
if(MOVE_X<-6)MOVE_X=-6;
if(MOVE_Y>+6)MOVE_Y=+6;
if(MOVE_Y<-6)MOVE_Y=-6;
//说明：在你的无人机控制部分叠加 光流量 MOVE_X，MOVE_Y即可,例如下面两句代码
// pidPitch.measured = Angle.pitch+MOVE_X; // 外环测量值 单位：角度 叠加 光流量 // MOVE_X
// pidRoll.measured = Angle.roll+MOVE_Y; //叠加 光流量MOVE_Y
/
//需要用到的光流函数
//********************************光流定点速度环****************************************************************//
float Move_X_velocity (float Gyro,float Target)
{
static float Bias,Last_Bias,Pwm,Integral_bias;
Bias=Gyro-Target; //计算偏差
if(Bias>150)Bias=150;
if(Bias<-150)Bias=-150;
if(real_height>10)Integral_bias+=Bias; //求出偏差的积分
if(Integral_bias>7000)Integral_bias=7000;
if(Integral_bias<-7000)Integral_bias=-7000;
Pwm=move_KP*Bias/1000+move_KI*Integral_bias/70000+move_KD*(Bias-Last_Bias)/1000; //位置式PID控制器
Last_Bias=Bias;
if(Flag_Stop==1)Integral_bias=0,Pwm=0,Bias=0,Last_Bias=0;
return Pwm; //增量输出
}
float Move_Y_velocity (float Gyro,float Target)
{
static float Bias,Last_Bias,Pwm,Integral_bias;
Bias=Gyro-Target; //计算偏差
if(Bias>150)Bias=150;
if(Bias<-150)Bias=-150;
if(real_height>10)Integral_bias+=Bias; //求出偏差的积分
if(Integral_bias>7000)Integral_bias=7000;
if(Integral_bias<-7000)Integral_bias=-7000;
Pwm=move_KP*Bias/1000+move_KI*Integral_bias/70000+move_KD*(Bias-Last_Bias)/1000; //位置式PID控制器
Last_Bias=Bias;
if(Flag_Stop==1)Integral_bias=0,Pwm=0,Bias=0,Last_Bias=0;
return Pwm; //增量输出
}
//********************************光流定点位置环****************************************************************//
float Move_X_position (float Gyro,float Target)
{
static float Position_PWM,Last_Position,Position_Bias,Position_Differential;
static float Position_Least;
Position_Least = Target-Gyro; //===获取偏差
Position_Bias *=0.8;
Position_Bias += Position_Least*0.2; //===一阶低通滤波器
Position_Differential = Position_Bias-Last_Position; //===获取偏差变化率
Last_Position = Position_Bias; //保存上一次的偏差
Position_PWM = Position_Bias*Position_Kp + Position_Differential*Position_Kd/10; //===位置控制
if(Flag_Stop==1)Position_Bias=0,Position_Differential=0,Position_Least=0,Last_Position=0,Position_PWM=0;
return Position_PWM; //增量输出
}
float Move_Y_position (float Gyro,float Target)
{
static float Position_PWM,Last_Position,Position_Bias,Position_Differential;
static float Position_Least;
Position_Least =Target-Gyro; //===获取偏差
Position_Bias *=0.8;
Position_Bias += Position_Least*0.2; //===一阶低通滤波器
Position_Differential = Position_Bias-Last_Position; //===获取偏差变化率
Last_Position = Position_Bias; //保存上一次的偏差
Position_PWM = Position_Bias*Position_Kp + Position_Differential*Position_Kd/10; //===位置控制
if(Flag_Stop==1)Position_Bias=0,Position_Differential=0,Position_Least=0,Last_Position=0,Position_PWM=0;
return Position_PWM; //增量输出
}
/
