2.2[Lib]ardupilot设备驱动实现方式

前言

之前看了APM的驱动（mpu6000、sr-04等）的库（srv/driver）基本上是各种class，并且也尝试按照这种格式编写了Xsens的驱动，这次需要编写应用层的代码，就顺藤摸瓜的从上面梳理，结果从ins.update()的入口进去后发现居然有变化，而且从nsh的终端上/dev下也没有看到相关的sensor，这不由得让我疑虑之前的思考方式；

顶层入口

在APM的代码里面，主要的就是::scheduler_tasks[]来实现整个功能的，不管是GPS、还是各种算法都是从这里入口进去，所以先来分析一个和我之前写的驱动能衔接上的；

GPS

 首先直接进入由调度器调度GPS的入口：

void Rover::update_GPS_50Hz(void)
{
  static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
  gps.update();//这里进行了GPS数据更新

  for (uint8_t i=0; i < gps.num_sensors(); i++) {
  if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
  last_gps_reading[i] = gps.last_message_time_ms(i);
  if (should_log(MASK_LOG_GPS)) {
       DataFlash.Log_Write_GPS(gps, i);
  }
  }
  }
}

这段程序主要做的是跟新GPS信息、log GPS信息、对获取的GPS信息判断并处理（只是为了有逻辑的运行，并没必要详细阅读）。重点是gps.update();这里可以看见gps.update();是更新的入口，进一步往下：

/*
  update all GPS instances
 */
void
AP_GPS::update(void)
{
  for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
  update_instance(i);//更新每一个GPS实例，有几个更新几个
  }

  // work out which GPS is the primary, and how many sensors we have
  for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
  if (state[i].status != NO_GPS) {
  num_instances = i+1;
  }
  if (_auto_switch) {
  if (i == primary_instance) {
  continue;
  }
  if (state[i].status > state[primary_instance].status) {
  // we have a higher status lock, change GPS
  primary_instance = i;
  continue;
  }

  bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);

  if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {

  uint32_t now = AP_HAL::millis();
  bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);

  if ( (another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
  (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000 ) ) {
  // this GPS has more satellites than the
  // current primary, switch primary. Once we switch we will
  // then tend to stick to the new GPS as primary. We don't
  // want to switch too often as it will look like a
  // position shift to the controllers.
  primary_instance = i;
  _last_instance_swap_ms = now;
  }
  }
  } else {
  primary_instance = 0;
  }
  }

  // update notify with gps status. We always base this on the primary_instance
  AP_Notify::flags.gps_status = state[primary_instance].status;
  AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
}

接下来的是一个选取哪个GPS作为起作用的GPS的算法。update_instance(i)；继续往下：

/*
  update one GPS instance. This should be called at 10Hz or greater
 */
void
AP_GPS::update_instance(uint8_t instance)
{
  if (_type[instance] == GPS_TYPE_HIL) {
  // in HIL, leave info alone
  return;
  }
  if (_type[instance] == GPS_TYPE_NONE) {
  /