四旋翼无人机光流定位和激光定高3（3中含激光定高原代码）

通常我们买到的激光测距模块如下：

 

首先在stm32 工程中加入vl53l0x.h 和 vl53l0x.c (文章后有下载链接)

int main(void)
{    
    cycleCounterInit();  //得到系统每个us的系统CLK个数，为以后延时函数，和得到精准的当前执行时间使用
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //2个bit的抢占优先级，2个bit的子优先级
    SysTick_Config(SystemCoreClock / 1000);    //系统滴答时钟

    ALL_Init();//系统初始化
  
    vl53l0xSetParam();              //初始化VL53L0X初始化  //*室内（无红外光线）长距离模式， <2m  ,33ms 周期*/
    
      

    while(1)
    {
          ANTO_polling();     //匿名上位机
            PilotLED();             //LED刷新
    }
}

control.c

    //激光定高 
         Remoter_Height=Remote.thr;
        
                if(Flag_4==1)Remoter_Height=-20;//电池电量过低自动降落      
                                            
                                        if(Remoter_Height>300&&Remoter_Height<=400)Remoter_Height=350;
                                        else if(Remoter_Height>400&&Remoter_Height<=500)Remoter_Height=450;
                                        else if(Remoter_Height>500&&Remoter_Height<=600)Remoter_Height=550;    
                                        else if(Remoter_Height>600&&Remoter_Height<=700)Remoter_Height=650;
                                        else if(Remoter_Height>700&&Remoter_Height<=800)Remoter_Height=750;
                                        else if(Remoter_Height>800&&Remoter_Height<=900)Remoter_Height=850;
                                        else if(Remoter_Height>900&&Remoter_Height<=1000)Remoter_Height=950;
                                        else if(Remoter_Height>1000&&Remoter_Height<=1100)Remoter_Height=1050;
                                        else if(Remoter_Height>1100&&Remoter_Height<=1200)Remoter_Height=1150;
                                        else if(Remoter_Height>1200&&Remoter_Height<=1300)Remoter_Height=1250;
                                        else if(Remoter_Height>1300&&Remoter_Height<=1400)Remoter_Height=1350;      
                            Height_=Remoter_Height;
                                   
        if((Height_-Final_Target)>0&&real_height>=200 && Flag_Stop==0)Final_Target+=0.3;
              if((Height_-Final_Target)>0&&real_height< 200 && Flag_Stop==0)Final_Target+=2.0;
        if((Height_-Final_Target)<0&&real_height>=200 && Flag_Stop==0)Final_Target-=0.4;     
        if((Height_-Final_Target)<0&&real_height< 200 && Flag_Stop==0)Final_Target-=2.0;             
                    if(Final_Target>1400)Final_Target=1400;//高度限制     
        if(Final_Target<-10&&real_height<60&&Flag_Stop==0) Flag_Stop=1;
        //***************************************激光定高******************************************//
        Flag_1++;
                    if(Flag_1==3)
        {
         Flag_1=0;
         data_25=vl53l0xReadRangeContinuousMillimeters();//15毫秒获取一次激光数据
         FILTER_1+=(data_25-FILTER_1)*0.2;  //低通滤波
         Distance=Mean_Filter((FILTER_1-8)); //平滑滤波，8为校准值
         square1=tan((Roll-Roll_Y)*angle_to_rad);
                     square2=tan((Pitch-Pitch_X)*angle_to_rad);                                
               square_AND=(square1*square1+square2*square2)+1;
               square_root=(Distance*Distance)/square_AND;                                
         real_height=1/invSqrt(square_root);                                                                                                       
        } 

         
                     Height=Height_Control(real_height,Final_Target);//高度PID    
        
         this_Final_Distance=Mean__Filter(real_height);

                                                                                     

        //调试时 this_Final_Distance暂时赋值为1000，即1米的高度
     //real_height=400;
         //this_Final_Distance=500;

//激光定高函数
/**************************************************************************
函数功能：测距值滤波
入口参数：测距值
返回  值：滤波后的测距值
**************************************************************************/
s16 Mean_Filter(u32 distance)
{
  u8 i;    
          u32 Sum_Speed = 0; 
    u32 Filter_Speed= 0; 
  static  s16 Speed_Buf[FILTERING_TIMES+1]={0};
       
  if(distance>0&&distance<2000)
           
   Speed_Buf[FILTERING_TIMES] = distance;
              for(i = 0; i < FILTERING_TIMES; i++) 
               {
                Speed_Buf[i] = Speed_Buf[i + 1];      
                Sum_Speed  += Speed_Buf[i];
               }
                Filter_Speed = (s16)(Sum_Speed / FILTERING_TIMES);
    
         
      return Filter_Speed;
           
}

static float invSqrt(float number) //返回平方根的倒数
{ 
  volatile long i; 
  volatile float x, y; 
  volatile const float f = 1.5F; 
    x = number * 0.5F; 
    y = number; i = * (( long * ) &y); 
    i = 0x5f375a86 - ( i >> 1 ); 
          y = * (( float * ) &i); 
          y = y * ( f - ( x * y * y ) ); 
          return y; 
}

float Height_Control (float distance,float Target)  //激光定高PID
{     
     static float error,Bias,Pwm,Last_Bias,Integral_bias;
    
     Bias=Target-distance; //计算偏差 
     if(real_height<200&&Bias<-70)Bias=-70;
           if(real_height>200&&Bias<-40)Bias=-40;
     if(real_height>=300&&Bias>+130)Bias=+130;
           if(real_height<300&&Bias>+200)Bias=+200;    
     if(Power>(Power1-10))Integral_bias+=Bias;     //求出偏差的积分
     if(Integral_bias>500000)Integral_bias=500000;
     if(Integral_bias<-500000)Integral_bias=-500000;   
           error=Bias-Last_Bias;
           if(error>10)error=10;
           if(error<-10)error=-10;       
      Pwm=Height_KP*Bias/150+Height_KI*Integral_bias/50000+Height_KD*error/130; //位置式PID控制器
if(Flag_Stop==1)Last_Bias=0,Integral_bias=0;    
     Last_Bias=Bias;      //保存上一次偏差 

     return Pwm;          //增量输出
}

/**************************************************************************
函数功能：测距值滤波
入口参数：测距值
返回  值：滤波后的测距值
**************************************************************************/
s16 Mean__Filter(u32 distance)
{
  u8 i;    
          u32 Sum_Speed = 0; 
    u32 Filter_Speed= 0; 
  static  s16 Speed_Buf[20+1]={0};
       
  if(distance>10&&distance<2000)
        
   Speed_Buf[20] = distance;
              for(i = 0; i < 20; i++) 
               {
                Speed_Buf[i] = Speed_Buf[i + 1];      
                Sum_Speed  += Speed_Buf[i];
               }
                Filter_Speed = (s16)(Sum_Speed / 20);

      return Filter_Speed;
           
}

首先在stm32 工程中加入vl53l0x.h 和 vl53l0x.c 

链接：https://pan.baidu.com/s/1uGtXnkLddEm6g2Xg9oMaBg 
提取码：abcd 
--来自百度网盘超级会员V4的分享