2020电赛D题绕组飞行器

在准备电赛的过程中，做了一下去年的题，本文将介绍我的方案及部分代码，希望可以帮助到大家。

一、我的装备

由于初学飞控所以主控用的是匿名的拓空者，还有匿名的光流传感器，北醒的激光雷达，星瞳科技的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;//设备I2C数据参数
VL53L0X_DeviceInfo_t vl53l0x_dev_info;//设备ID版本信息
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);//Ref参考校准

	 Delay_ms(2);
	 status = VL53L0X_PerformRefSpadManagement(dev, &refSpadCount, &isApertureSpads);//执行参考SPAD管理
	 
                 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);//使能SIGMA范围检查
	 
	 Delay_ms