在准备电赛的过程中,做了一下去年的题,本文将介绍我的方案及部分代码,希望可以帮助到大家。
一、我的装备
由于初学飞控所以主控用的是匿名的拓空者,还有匿名的光流传感器,北醒的激光雷达,星瞳科技的OPENMV和正点原子的激光测距模块ATK-VL53L0X。
二、实现思路
1、一键起飞,然后自旋找到第一个杆。
2、找到第一个杆后,由于激光测距模块最远测距是2m,实际更短,所以还测不到距离,因此要先缓慢靠近杆,直到激光测距模块有效,再进行PID控制与杆的位置,期间始终有OPENMV的角度调节。
3、等到角度和距离都合适开始绕杆,通过航向角控制旋转角度
4、绕完第一个杆后边后退,自旋寻找第二个杆。
5、找到后重复绕第一个杆的过程
6、寻找降落点然后降落(我没有实现这个任务)
三、代码介绍
1、下面这一段代码是ATK-VL53L0X的控制程序,由正点原子的测试例程修改,具体的模块介绍可以看正点原子的使用手册。
#include "vl53l0x.h"
#include "vl53l0x_i2c.h"
VL53L0X_Dev_t vl53l0x_dev;
VL53L0X_DeviceInfo_t vl53l0x_dev_info;
VL53L0X_RangingMeasurementData_t vl53l0x_data;
vu16 vl53l0x_pdata[10];
mode_data Mode_data[]=
{
{(FixPoint1616_t)(0.25*65536),
(FixPoint1616_t)(18*65536),
33000,
14,
10},
{(FixPoint1616_t)(0.25*65536) ,
(FixPoint1616_t)(18*65536),
200000,
14,
10},
{(FixPoint1616_t)(0.1*65536) ,
(FixPoint1616_t)(60*65536),
33000,
18,
14},
{(FixPoint1616_t)(0.25*65536) ,
(FixPoint1616_t)(32*65536),
20000,
14,
10},
};
VL53L0X_Error vl53l0x_start_single_test(VL53L0X_Dev_t *dev,VL53L0X_RangingMeasurementData_t *pdata,char *buf)
{
static u8 i=0;
static vu32 sum=0;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_GetRangingMeasurementData(dev,pdata);
if(pdata->RangeMilliMeter<2000)
{
sum+=pdata->RangeMilliMeter;
i++;
if(i==5)
{
opmv_gan_ctrl.Juli=sum/5;
sum=0;
i=0;
}
}
return status;
}
VL53L0X_Error vl53l0x_set_mode(VL53L0X_Dev_t *dev,u8 mode)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t VhvSettings;
uint8_t PhaseCal;
uint32_t refSpadCount;
uint8_t isApertureSpads;
status = VL53L0X_StaticInit(dev);
status = VL53L0X_PerformRefCalibration(dev, &VhvSettings, &PhaseCal);
Delay_ms(2);
status = VL53L0X_PerformRefSpadManagement(dev, &refSpadCount, &isApertureSpads);
Delay_ms(2);
status = VL53L0X_SetDeviceMode(dev,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
Delay_ms(2);
status = VL53L0X_SetInterMeasurementPeriodMilliSeconds(dev,Mode_data[mode].timingBudget);
Delay_ms(2);
status = VL53L0X_SetLimitCheckEnable(dev,VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,1);
Delay_ms(2);
status = VL53L0X_SetLimitCheckEnable(dev,VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,1);
Delay_ms(2);
status = VL53L0X_SetLimitCheckValue(dev,VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,Mode_data[mode].sigmaLimit);
Delay_ms(2);
status = VL53L0X_SetLimitCheckValue(dev,VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,Mode_data[mode].signalLimit);
Delay_ms(2);
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(dev,Mode_data[mode].timingBudget);
Delay_ms(2);
status = VL53L0X_SetVcselPulsePeriod(dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, Mode_data[mode].preRangeVcselPeriod);
Delay_ms(2);
status = VL53L0X_SetVcselPulsePeriod(dev, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, Mode_data[mode].finalRangeVcselPeriod);
VL53L0X_StartMeasurement(dev);
return status;
}
VL53L0X_Error vl53l0x_init(VL53L0X_Dev_t *dev)
{
GPIO_InitTypeDef GPIO_InitStructure;
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
VL53L0X_Dev_t *pMyDevice = dev;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
pMyDevice->I2cDevAddr = VL53L0X_Addr;
pMyDevice->comms_type = 1;
pMyDevice->comms_speed_khz = 400;
VL53L0X_i2c_init();
GPIO_ResetBits(GPIOB,GPIO_Pin_0);
Delay_ms(30);
GPIO_SetBits(GPIOB,GPIO_Pin_0);
Delay_ms(30);
Status = VL53L0X_DataInit(pMyDevice);
Delay_ms(2);
Status = VL53L0X_GetDeviceInfo(pMyDevice,&vl53l0x_dev_info);
return Status;
}
void vl53l0x_Init(void)
{
while(vl53l0x_init(&vl53l0x_dev)) ;
while(vl53l0x_set_mode(&vl53l0x_dev,3));
}
void vl53l0x_Task(u8 dT_ms)
{
static char buf[VL53L0X_MAX_STRING_LENGTH];
vl53l0x_start_single_test(&vl53l0x_dev,&vl53l0x_data,buf);
}
2、下面是主要的控制函数,结构是模仿匿名的巡线程序写的
#include "Gan_Ctrl.h"
#include "Drv_OpenMV.h"
#include "Ano_OPMV_Ctrl.h"
#include "Ano_Math.h"
#include "Ano_Filter.h"
#include "ANO_IMU.h"
#include "Ano_FlightCtrl.h"
#include "vl53l0x.h"
#include "Drv_usart.h"
#include "Ano_FlyCtrl.h"
_opmv_gan_ctrl_st opmv_gan_ctrl;
Ldc_gan_PID Gan_PID;
#define FORWARD_VEL 12
#define Ganyspeed 6
#define Ganyawspeed 90
static u8 step;
float Color=1;
unsigned char Uartsendbuff[7]={0xaa,0xaf,0x05,0x01,0x06,0x03,0x68};
void Ldc_pid_Init(void)
{
Gan_PID.Kp=20;
Gan_PID.Kd=5;
Gan_PID.Expect=500;
opmv_gan_ctrl.Ganyaw=0;
opmv_gan_ctrl.Gannumber=0;
opmv.gan.sta=0;
opmv_gan_ctrl.target_loss=1;
}
void Ldc_gan_Ctrl(void)
{
Gan_PID.Out=0;
Gan_PID.Now=(float)opmv_gan_ctrl.Juli;
Gan_PID.P=Gan_PID.Now-Gan_PID.Expect;
Gan_PID.D=Gan_PID.Old-Gan_PID.Now;
Gan_PID.Out=Gan_PID.Kp*Gan_PID.P+Gan_PID.Kd*Gan_PID.D;
Gan_PID.Out =0.001f*Gan_PID.Out;
Gan_PID.Old =Gan_PID.Now;
}
void Ldcqiehuan(void)
{
static u8 x=0;
if(opmv_gan_ctrl.Gannumber==0)
{
opmv_gan_ctrl.Gannumber=1;
opmv_gan_ctrl.Ganyaw=imu_data.yaw+200*Color;
if(opmv_gan_ctrl.Ganyaw<-180) opmv_gan_ctrl.Ganyaw+= 360;
else if(opmv_gan_ctrl.Ganyaw>180) opmv_gan_ctrl.Ganyaw -= 360;
}
else if((opmv_gan_ctrl.Gannumber==1)&&( (opmv_gan_ctrl.Ganyaw-imu_data.yaw<35)&&(opmv_gan_ctrl.Ganyaw-imu_data.yaw>-35)))
{
opmv_gan_ctrl.Gannumber=2 ;
opmv_gan_ctrl.Ganyaw=imu_data.yaw+220*Color;
if(opmv_gan_ctrl.Ganyaw<-180) opmv_gan_ctrl.Ganyaw+= 360;
else if(opmv_gan_ctrl.Ganyaw>180) opmv_gan_ctrl.Ganyaw -= 360;
}
else if((opmv_gan_ctrl.Gannumber==2)&&( (opmv_gan_ctrl.Ganyaw-imu_data.yaw<35)&&(opmv_gan_ctrl.Ganyaw-imu_data.yaw>-35)))
{
opmv_gan_ctrl.Gannumber=3;
Usart3_Send(Uartsendbuff,7);
}
else if(opmv_gan_ctrl.Gannumber==3)
{
if(x==0)
{
step=2;
x=1;
Color=-1;
opmv_gan_ctrl.Gannumber=0;
}
else
{
MyFlyCtrl(0x02,0,0);
}
}
}
void Ldc_gan_StepProcedure(u8 *dT_ms)
{
static u8 time=0;
switch (step)
{
case 0:{
opmv_gan_ctrl.exp_yaw_pal_dps = 0;
opmv_gan_ctrl.exp_velocity_h_cmps[0]=0;
opmv_gan_ctrl.exp_velocity_h_cmps[1]=0;
time=0;
step=1;
}break;
case 1:{
if(opmv.gan.sta==1)
{
opmv_gan_ctrl.exp_yaw_pal_dps=opmv.gan.ganx;
step=3;
}
else
{
if(opmv_gan_ctrl.Gannumber==0)
{
opmv_gan_ctrl.exp_yaw_pal_dps=(-15);
}
else
{
if(time<=100)
{
time+=*dT_ms;
}
else
{
opmv_gan_ctrl.exp_yaw_pal_dps =(-15)*Color;
time=0;
}
}
}
}break;
case 2:{
opmv_gan_ctrl.exp_velocity_h_cmps[0]=-16;
opmv_gan_ctrl.exp_yaw_pal_dps=(-4);
if(time<200)
{
time+=*dT_ms;
}
else
{
time=0;
opmv_gan_ctrl.exp_velocity_h_cmps[0]=0;
step=0;
opmv_gan_ctrl.Gannumber=0;
}
}break;
case 3:{
if(opmv_gan_ctrl.Juli<=550&&opmv_gan_ctrl.Juli>=450)
{
step=4;
Ldcqiehuan();
opmv_gan_ctrl.exp_velocity_h_cmps[0]=Gan_PID.Out;
}
else
{
if(opmv_gan_ctrl.Gannumber==0)
{
opmv_gan_ctrl.exp_velocity_h_cmps[0] =FORWARD_VEL;
if(time<80)
{
time+=*dT_ms;
}
else
{
time=0;
}
}
else
{
step=4;
}
}
}break;
case 4:{
if((opmv.gan.ganx<=20)&&(opmv.gan.ganx>=-20))
{
opmv_gan_ctrl.exp_velocity_h_cmps[0]=Gan_PID.Out;
opmv_gan_ctrl.exp_velocity_h_cmps[1] =Ganyspeed*Color;
if(time<300)
{
time+=*dT_ms;
}
else
{
opmv_gan_ctrl.exp_velocity_h_cmps[1]=0;
step=5;
time=0;
}
}
else
{
opmv_gan_ctrl.exp_yaw_pal_dps =opmv.gan.ganx;
}
}break;
case 5:{
opmv_gan_ctrl.exp_velocity_h_cmps[0]=Gan_PID.Out;
opmv_gan_ctrl.exp_yaw_pal_dps=Ganyawspeed*Color;
if(time<250)
{
time+=*dT_ms;
}
else
{
opmv_gan_ctrl.exp_yaw_pal_dps=0;
step=1;
time=0;
}
}break;
default:{
}break;
}
}
void Gan_Ctrl(u8 *dT_ms,u8 en)
{
if(en)
{
Ldc_gan_Ctrl();
if(opmv.gan.sta == 1)
{
opmv_gan_ctrl.exp_yaw_pal_dps =opmv.gan.ganx;
opmv_gan_ctrl.target_loss=0;
}
else
{
opmv_gan_ctrl.target_loss=1;
opmv_gan_ctrl.exp_velocity_h_cmps[0]=0;
opmv_gan_ctrl.exp_velocity_h_cmps[1] = 0;
opmv_gan_ctrl.exp_yaw_pal_dps =0;
}
if(opmv_gan_ctrl.Gannumber!=0)
{
Ldcqiehuan();
}
Ldc_gan_StepProcedure(dT_ms);
opmv_gan_ctrl.exp_yaw_pal_dps = LIMIT(opmv_gan_ctrl.exp_yaw_pal_dps,-90,90);
}
else
{
opmv_gan_ctrl.exp_velocity_h_cmps[0] = 0;
opmv_gan_ctrl.exp_velocity_h_cmps[1] = 0;
opmv_gan_ctrl.exp_yaw_pal_dps = 0;
step=0;
}
}
本人水平有限,此代码仅供参考,如果要做找降落点的任务可以修Ldcqiehuan()函数,大佬们有好的方法或者认为我的代码有可以优化的地方的欢迎留言,我变量的命名实在是比较随便。
最终效果可以看我在b站的视频:https://www.bilibili.com/video/BV1o64y1y7Em
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)