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 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) {
  // not enabled
  state[instance].status = NO_GPS;
  state[instance].hdop = 9999;
  return;
  }
  if (locked_ports & (1U<read();//这里读取GPS的入口,这里特别需要注意
  uint32_t tnow = AP_HAL::millis();

  // if we did not get a message, and the idle timer of 2 seconds
  // has expired, re-initialise the GPS. This will cause GPS
  // detection to run again
  if (!result) {
  if (tnow - timing[instance].last_message_time_ms > 4000) {
  // free the driver before we run the next detection, so we
  // don't end up with two allocated at any time
  delete drivers[instance];
  drivers[instance] = nullptr;
  memset(&state[instance], 0, sizeof(state[instance]));
  state[instance].instance = instance;
  state[instance].status = NO_GPS;
  state[instance].hdop = 9999;
  timing[instance].last_message_time_ms = tnow;
  }
  } else {
  timing[instance].last_message_time_ms = tnow;
  if (state[instance].status >= GPS_OK_FIX_2D) {
  timing[instance].last_fix_time_ms = tnow;
  }
  }
}

这段代码bool result =drivers[instance]->read();是实现数据跟新的函数，之后有一个计时，如果2s没收到数据则重新初始化GPS。这里read是class AP_GPS_Backend的虚函数，这里需要划个重点，就是apm在实现传感器设备的驱动时候提出了一个前后端的概念：
啥意思呢，就是说APM会为这个传感器设备定义一个前段class用来提供数据给上层应用数据；同时也会存在一个后端class用来实现读取传感器的值；
需要特别注意的是APM自带的传感器驱动都会存在libraries/AP_xxx下，通过AP_xxx_PX4.cpp来实现对PX4驱动的复用；
问题就来了，APM在实现这个后端class的时候，读取传感器的方式有两种，下面挨个介绍：

GPS这种是第一种，也就是我之前写驱动理解的那种；继续。。。
public AP_GPS_Backend这个后端的class会被AP_GPS_PX4公共继承，所以里面也就会有read方法的实现：

// update internal state if new GPS information is available
bool
AP_GPS_PX4::read(void)
{
  bool updated = false;
  orb_check(_gps_sub, &updated);

  if (updated) {
  if (OK == orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos)) {//这里清晰可见采用了orb的方式从发布者那里获取到了数据
  state.last_gps_time_ms = AP_HAL::millis();
  state.status = (AP_GPS::GPS_Status) (_gps_pos.fix_type | AP_GPS::NO_FIX);
  state.num_sats = _gps_pos.satellites_used;
  state.hdop = uint16_t(_gps_pos.eph*100.0f + .5f);

  if (_gps_pos.fix_type >= 2) {
  state.location.lat = _gps_pos.lat;
  state.location.lng = _gps_pos.lon;
  state.location.alt = _gps_pos.alt/10;

  state.ground_speed = _gps_pos.vel_m_s;
  state.ground_course = wrap_360(degrees(_gps_pos.cog_rad));
  state.hdop = _gps_pos.eph*100;

  // convert epoch timestamp back to gps epoch - evil hack until we get the genuine
  // raw week information (or APM switches to Posix epoch ;-) )
  const uint64_t posix_offset = 3657ULL * 24 * 3600 * 1000 - GPS_LEAPSECONDS_MILLIS;
  const uint64_t ms_per_week = 7ULL * 24 * 3600 * 1000;
  uint64_t epoch_ms = _gps_pos.time_utc_usec/1000;
  uint64_t gps_ms = epoch_ms - posix_offset;
  state.time_week = (uint16_t)(gps_ms / ms_per_week);
  state.time_week_ms = (uint32_t)(gps_ms - (state.time_week) * ms_per_week);

  if (_gps_pos.time_utc_usec == 0) {
  // This is a work-around for https://github.com/PX4/Firmware/issues/1474
  // reject position reports with invalid time, as APM adjusts it's clock after the first lock has been acquired
  state.status = AP_GPS::NO_FIX;
  }
  }
  if (_gps_pos.fix_type >= 3) {
  state.have_vertical_velocity = _gps_pos.vel_ned_valid;
  state.velocity.x = _gps_pos.vel_n_m_s;
  state.velocity.y = _gps_pos.vel_e_m_s;
  state.velocity.z = _gps_pos.vel_d_m_s;
  state.speed_accuracy = _gps_pos.s_variance_m_s;
  state.have_speed_accuracy = true;
  }
  else {
  state.have_vertical_velocity = false;
  }
  }
  }

  return updated;
}

采用了orb的方式来订阅了消息，下半部分也就是src/driver下面就针对性的写了GPS的对应驱动，这和我之前写xsens的方法是一样的；
那么问题来了，这第二中是怎样的呢？这个就要从ins的更新说起；

AHRS

还是从顶层代码入手SCHED_TASK(ahrs_update, 50, 6400),这里是对ahrs的更新，同样我们继续往下：

// update AHRS system
void Rover::ahrs_update()
{
  update_soft_armed();

#if HIL_MODE != HIL_MODE_DISABLED
  // update hil before AHRS update
  gcs_update();
#endif

  // when in reverse we need to tell AHRS not to assume we are a
  // 'fly forward' vehicle, otherwise it will see a large
  // discrepancy between the mag and the GPS heading and will try to
  // correct for it, leading to a large yaw error
  ahrs.set_fly_forward(!in_reverse);

  ahrs.update();//这里实现AHRS的更新

  // if using the EKF get a speed update now (from accelerometers)
  Vector3f velocity;
  if (ahrs.get_velocity_NED(velocity)) {
  ground_speed = norm(velocity.x, velocity.y);
  }

  if (should_log(MASK_LOG_ATTITUDE_FAST)) {
  Log_Write_Attitude();
  }

  if (should_log(MASK_LOG_IMU)) {
  DataFlash.Log_Write_IMU(ins);
  }
}

因为AP_AHRS_DCM是公共继承AHRS的，所以继续AP_AHRS_DCM.update();往下：
// run a full DCM update round
void
AP_AHRS_DCM::update(void)
{
  float delta_t;

  if (_last_startup_ms == 0) {
  _last_startup_ms = AP_HAL::millis();
  }

  // tell the IMU to grab some data
  _ins.update();//这里进行了ins更新

  // ask the IMU how much time this sensor reading represents
  delta_t = _ins.get_delta_time();

  // if the update call took more than 0.2 seconds then discard it,
  // otherwise we may move too far. This happens when arming motors
  // in ArduCopter
  if (delta_t > 0.2f) {
  memset(&_ra_sum[0], 0, sizeof(_ra_sum));
  _ra_deltat = 0;
  return;
  }

  // Integrate the DCM matrix using gyro inputs
  matrix_update(delta_t);

  // Normalize the DCM matrix
  normalize();

  // Perform drift correction
  drift_correction(delta_t);

  // paranoid check for bad values in the DCM matrix
  check_matrix();

  // Calculate pitch, roll, yaw for stabilization and navigation
  euler_angles();

  // update trig values including _cos_roll, cos_pitch
  update_trig();
}

/*
  update gyro and accel values from backends
 */
void AP_InertialSensor::update(void)
{
  // during initialisation update() may be called without
  // wait_for_sample(), and a wait is implied
  wait_for_sample();

  if (!_hil_mode) {
  for (uint8_t i=0; iupdate();//这里就是重点了
  }

  // clear accumulators
  for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
  _delta_velocity_acc[i].zero();
  _delta_velocity_acc_dt[i] = 0;
  _delta_angle_acc[i].zero();
  _delta_angle_acc_dt[i] = 0;
  }

  if (!_startup_error_counts_set) {
  for (uint8_t i=0; i 2000) {
  _startup_error_counts_set = true;
  }
  }

  for (uint8_t i=0; i _gyro_startup_error_count[i] && have_zero_gyro_error_count) {
  // we prefer not to use a gyro that has had errors
  _gyro_healthy[i] = false;
  }
  if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) {
  // we prefer not to use a accel that has had errors
  _accel_healthy[i] = false;
  }
  }

  // set primary to first healthy accel and gyro
  for (uint8_t i=0; i

_backends就是之前提到的前后端的后端class，但是这个后端class却不是按照之前的驱动实现方式来获取传感器的值的，同样我们找到了继承后端的类的实现class AP_InertialSensor_Invensense : public

AP_InertialSensor_Backend

在AP_InertialSensor_Invensense也就是mpu6000中，驱动的实现方式是这样的：

void AP_InertialSensor_Invensense::_read_fifo()
{
  uint8_t n_samples;
  uint16_t bytes_read;
  uint8_t *rx = _fifo_buffer;
  bool need_reset = false;

  if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
  goto check_registers;
  }

  bytes_read = uint16_val(rx, 0);
  n_samples = bytes_read / MPU_SAMPLE_SIZE;

  if (n_samples == 0) {
  /* Not enough data in FIFO */
  goto check_registers;
  }

  /*
  testing has shown that if we have more than 32 samples in the
  FIFO then some of those samples will be corrupt. It always is
  the ones at the end of the FIFO, so clear those with a reset
  once we've read the first 24. Reading 24 gives us the normal
  number of samples for fast sampling at 400Hz
  */
  if (n_samples > 32) {
  need_reset = true;
  n_samples = 24;
  }

  while (n_samples > 0) {
  uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN);
  if (!_dev->set_chip_select(true)) {
  if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
  goto check_registers;
  }
  } else {
  // this ensures we keep things nicely setup for DMA
  uint8_t reg = MPUREG_FIFO_R_W | 0x80;
  if (!_dev->transfer(®, 1, nullptr, 0)) {
  _dev->set_chip_select(false);
  goto check_registers;
  }
  memset(rx, 0, n * MPU_SAMPLE_SIZE);
  if (!_dev->transfer(rx, n * MPU_SAMPLE_SIZE, rx, n * MPU_SAMPLE_SIZE)) {
  hal.console->printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
  _dev->set_chip_select(false);
  goto check_registers;
  }
  _dev->set_chip_select(false);
  }

  if (_fast_sampling) {
  if (!_accumulate_fast_sampling(rx, n)) {
  debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);
  break;
  }
  } else {
  if (!_accumulate(rx, n)) {
  break;
  }
  }
  n_samples -= n;
  }

  if (need_reset) {
  //debug("fifo reset n_samples %u", bytes_read/MPU_SAMPLE_SIZE);
  _fifo_reset();
  }

check_registers:
  // check next register value for correctness
  _dev->set_speed(AP_HAL::Device::SPEED_LOW);
  if (!_dev->check_next_register()) {
  _inc_gyro_error_count(_gyro_instance);
  _inc_accel_error_count(_accel_instance);
  }
  _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}

bool AP_InertialSensor_Invensense::_block_read(uint8_t reg, uint8_t *buf,
  uint32_t size)
{
  return _dev->read_registers(reg, buf, size);
}

AP_HAL::OwnPtr _dev;这个_dev是之前我们梳理过的APM的抽象框架，这个设备就可以绕过之前我们编写xsens驱动的方式来直接读取传感器的值了，下面是这个dev的定义；
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr dev,
enum Rotation rotation = ROTATION_NONE);
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr dev,
enum Rotation rotation = ROTATION_NONE);

总结一下

HAL和单独的驱动区别有那些，共同的基础又由哪些:
1.不管是HAL还是单独存在src/drv下的驱动，他们都是基于nuttx这个RTOS的，所以这是它们唯一接触到硬件的接口；
2.即使是独立驱动，它也是通过继承AP_HAL::I2C或者AP_HAL::SPI等等来调用nuttx的驱动接口来获取传感器的数据的；
3.区别就在于两点：其一就是，单独驱动传输数据是需要通过orb来实现的，直接通过上面这种则更直接；其二就是，libraries下的AP_库是包含了数据的处理算法的，所以说，数据和驱动几乎就融合在了一起，包括对传感器的设置等等操作都包含在这里面；

说说优劣

显然采用直接的方式，可以去掉orb环节，保证数据的及时性；但是在代码的模块化程度上相对于采用orb的独立驱动就低了很多；