实现过程主要包含三个部分：
1.怎么初始化避障模块？
2.怎么获取避障距离数据？
3.怎么实现避障控制

1.如何初始化

(1)主函数中调用过这个函数：

  1. void Rover::init_ardupilot() 
  2. {  
  3.   …………    
  4.   init_proximity();//避障函数初始化 
  5. }

在/APMrover2/sensors里有：

  1. void Rover::init_proximity(void)
  2. {
  3.      #if PROXIMITY_ENABLED == ENABLED
  4.      g2.proximity.init();//测距传感器初始化
  5.      g2.proximity.set_rangefinder(&rangefinder);//设置近距传感器
  6.      #endif
  7. }

(2)避障初始化函数执行顺序
首先看 g2.proximity.init()函数;

  1. void AP_Proximity::init(void)
  2. {
  3.     if (num_instances != 0)  //没有传感器就直接返回
  4.         return;
  6.     for (uint8_t i=0; i<PROXIMITY_MAX_INSTANCES; i++) 
  7.     {
  8.         detect_instance(i); //传感器识别
  9.         if (drivers[i] != nullptr) //我们为这个实例加载了一个驱动程序，所以它必须存在（尽管它可能不健康）
  10.         {
  11.             // we loaded a driver for this instance, so it must be present (although it may not be healthy)
  12.             num_instances = i+1;
  13.         }
  14.         //初始化状态标志-----initialise status
  15.         state[i].status = Proximity_NotConnected;
  16.     }
  17. }
  18. ------------------------------------------------------------
  19. 下面代码是识别我们用的哪种传感器 detect_instance(i);
  20. ------------------------------------------------------------
  21. void AP_Proximity::detect_instance(uint8_t instance)
  22. {
  23.     uint8_t type = _type[instance];
  24.     if (type == Proximity_Type_SF40C)   //激光测距模块
  25.     {
  26.         if (AP_Proximity_LightWareSF40C::detect(serial_manager)) 
  27.         {
  28.             state[instance].instance = instance;
  29.             //使用哪种传感器就创建哪种对象：
  30.             drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], serial_manager);
  31.             return;
  32.         }
  33.     }
  34.     if (type == Proximity_Type_MAV) 
  35.     {
  36.         state[instance].instance = instance;
  37.         drivers[instance] = new AP_Proximity_MAV(*this, state[instance]);
  38.         return;
  39.     }
  40.     if (type == Proximity_Type_TRTOWER) 
  41.     {
  42.         if (AP_Proximity_TeraRangerTower::detect(serial_manager)) 
  43.         {
  44.             state[instance].instance = instance;
  45.             drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], serial_manager);
  46.             return;
  47.         }
  48.     }
  49.     if (type == Proximity_Type_RangeFinder) //测距仪器
  50.     {
  51.         state[instance].instance = instance;
  52.         drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);
  53.         return;
  54.     }
  55. #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
  56.     if (type == Proximity_Type_SITL) 
  57.     {
  58.         state[instance].instance = instance;
  59.         drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); //创建实例
  60.         return;
  61.     }
  62. #endif
  63. }

这是APM支持的驱动类型：
Ardupilot 避障代码 - 图1


在const AP_Param::GroupInfo AP_Proximity::var_info[]中有我们用到的参数**

2.更新避障数据

SCHEDTASK(update_proximity, 100, 50), //更新近距离传感器，避障使用

//更新数据的函数： 

  1. void Rover::update_proximity(void)
  2. {
  3.      #if PROXIMITY_ENABLED == ENABLED
  4.      g2.proximity.update();
  5.      #endif
  6. }

在update()中有这一行：drivers[i]->update(); //更新数据

  1. //这里由于 virtual void update() = 0;是纯虚函数，我们以AP_Proximity_RangeFinder为例分析
  2. void AP_Proximity_RangeFinder::update(void)
  3. {
  4.     //如果没有测距仪目标立即退出
  5.     const RangeFinder *rngfnd = frontend.get_rangefinder();
  6.     if (rngfnd == nullptr) 
  7.     {
  8.         set_status(AP_Proximity::Proximity_NoData);
  9.         return;
  10.     }
  12.     //查看所有测距仪
  13.     for (uint8_t i=0; i<rngfnd->num_sensors(); i++) 
  14.     {
  15.         if (rngfnd->has_data(i)) 
  16.         {
  17.             //检查水平测距仪
  18.             if (rngfnd->get_orientation(i) <= ROTATION_YAW_315) 
  19.             {
  20.                 uint8_t sector = (uint8_t)rngfnd->get_orientation(i);  //获取方向
  21.                 _angle[sector] = sector * 45;                          //获取角度
  22.                 _distance[sector] = rngfnd->distance_cm(i) / 100.0f;   //获取距离
  23.                 _distance_min = rngfnd->min_distance_cm(i) / 100.0f;   //最小距离
  24.                 _distance_max = rngfnd->max_distance_cm(i) / 100.0f;   //最大距离
  25.                 _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);  //是否在限制的范围
  26.                 _last_update_ms = AP_HAL::millis();                    //获取上次时间
  27.                 update_boundary_for_sector(sector);//这里是更新扇区的介绍，重要函数
  28.             }
  29.             //检查向上测距仪----------check upward facing range finder
  30.             if (rngfnd->get_orientation(i) == ROTATION_PITCH_90) 
  31.             {
  32.                 _distance_upward = rngfnd->distance_cm(i) / 100.0f;
  33.                 _last_upward_update_ms = AP_HAL::millis();
  34.             }
  35.         }
  36.     }
  38.     //检查超时并设置健康状态--------- check for timeout and set health status
  39.     if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS)) 
  40.     {
  41.         set_status(AP_Proximity::Proximity_NoData);
  42.     } else 
  43.     {
  44.         set_status(AP_Proximity::Proximity_Good);
  45.     }
  46. }

获取到的这些值应该传到避障控制函数中，
怎么传过去的，要看这个函数：update_boundary_for_sector(sector)//更新边界区域

这个函数最终会计算得到避障边界点数据：_boundary_point[]

3.避障控制实现

下面这个函数是控制航行器速度的：

  1. _avoid->adjust_velocity(_pos_control.get_pos_xy_kP(),_loiter_accel_cmss, desired_vel);

具体实现：

  1. void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel)
  2. {
  3.     //没有使能立即退出---exit immediately if disabled
  4.     if (_enabled == AC_AVOID_DISABLED) 
  5.     {
  6.         return;
  7.     }
  9.     //限制加速度------limit acceleration
  10.     float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
  12.     if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) //这个是围栏设置，可以先不看
  13.     {
  14.         adjust_velocity_circle_fence(kP, accel_cmss_limited, desired_vel);
  15.         adjust_velocity_polygon_fence(kP, accel_cmss_limited, desired_vel);
  16.     }
  18.     if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled) //采用近距传感器，重点看这里
  19.     {
  20.         adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel); //根据近距离传感器调节速度
  21.     }
  22. }

在这个函数adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel) 中有如下代码：

  1.      ……………………………………………………
  2.     //从近距传感器获取边界-----------get boundary from proximity sensor
  3.     uint16_t num_points;
  4.     const Vector2f *boundary = _proximity.get_boundary_points(num_points);//计算边界点
  5.     adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, false, _margin); //通过避障点计算所需要的目标速度。