Ardupilot源码分析（四）传感器驱动的前后端程序如何通信

lhl428

在第二篇文章中讲过 传感器驱动程序的前后端设计方法，这一篇文章我们详细分析一下它是如何实现的。这一系列文章都以ArduSub和Pixhawk1(狗骨头)进行说明，如果读者查看自己工程的源码，请先编译，否则有一些宏定义会找不到。

   以惯性导航器件为例，首先完成设备的初始化，然后再定时更新设备数据。由于总线结构问题，可能多个设备共用一条总线，需要给总线通信单独开辟一个OS线程,如spi bus总线，然后每个设备在这个线程中注册回调函数，以完成自身的定时刷新。
      飞控的应用层程序从上述传感器中取数据，为了使应用层不需要关心设备驱动层，Ardupilot设计了传感器驱动前后端程序结构，后端在上述线程中定时读取传感器原始数据并处理，应用程序从前端更新数据，前后端对象分别持有对方的指针，以便互通数据，为了兼容每款飞控板使用的不同型号的传感器，又分别根据具体型号对后端程序进行继承。
     INS前端Class                       INS后端Class(基类)                         INS具体型号Class
                                                                                                                   AP_InertialSensor_BMI055
AP_InertialSensor            AP_InertialSensor_Backend       AP_InertialSensor_Invensense
                                                                                                                   AP_InertialSensor_LSM9DS0

    这里我们仍以惯性器件来说明，在Ardupilot工程中就是ins对象，它被定义在Sub.h中。
    AP_Baro barometer;
    Compass compass;
    AP_InertialSensor ins;
    我们先来看一下AP_InertialSensor类的定义,去除类内定义的一些setXX getXX函数，剩余一些初始化、采样刷新、后端进程相关的精简程序如下。
//libraries/AP_InertialSensor/AP_InertialSensor.h
class AP_InertialSensor_Backend;
class AuxiliaryBus;
class AP_AHRS;

class AP_InertialSensor : AP_AccelCal_Client
{
friend class AP_InertialSensor_Backend;
public:
void init(uint16_t sample_rate_hz);
    // update gyro and accel values from accumulated samples
void update(void);
    // wait for a sample to be available
void wait_for_sample(void);
void detect_backends(void);
private:
    // load backend drivers
bool _add_backend(AP_InertialSensor_Backend *backend);
void _start_backends();
AP_InertialSensor_Backend *_find_backend(int16_t backend_id, uint8_t instance);
    // backend objects
    AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
    // Most recent accelerometer reading
    Vector3f _accel[INS_MAX_INSTANCES];//前后端数据交换数组
    // Most recent gyro reading
    Vector3f _gyro[INS_MAX_INSTANCES];//前后端数据交换数组
};

namespace AP {
AP_InertialSensor &ins();
};
ins的初始化

setup()
    init_ardupilot()
        startup_INS_ground()
            startup_INS_ground()
                ins.init(scheduler.get_loop_rate_hz());
    其中scheduler.get_loop_ratehz()是指main_loop函数的执行频率，ArduCopter和ArduSub中默认为400HZ，其他类型车辆默认50HZ。init函数原型如下。
//libraries/AP_InertialSensor/AP_InertialSensor.cpp
//精简代码
void AP_InertialSensor::init(uint16_t sample_rate)
{
//启动后端
if (_gyro_count == 0 && _accel_count == 0) {
_start_backends();
    }
}
    ins init初始化之前ACCEL和GYRO的数量都为0，所以先启动后端，在启动后端时会先检测有哪些后端，即根据板型确定板载的传感器有哪些，传感器的数量，进一步确定开启对应的后端。
启动后端

//精简代码
void AP_InertialSensor::_start_backends()
{
detect_backends();

for (uint8_t i = 0; i < _backend_count; i++) {
_backends->start();
    }
}
    精简版的函数定义如下。
/*detect available backends for this board*/
void AP_InertialSensor::detect_backends(void)
{
uint8_t probe_count = 0;
uint8_t enable_mask = 0x7F;
uint8_t found_mask = 0;

    /*use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends */
#define ADD_BACKEND(x) do { \
if (((1U<<probe_count)&enable_mask) && _add_backend(x)) { \
            found_mask |= (1U<<probe_count); \
        } \
        probe_count++; \
} while (0)

#if defined(HAL_INS_PROBE_LIST)
    // IMUs defined by IMU lines in hwdef.dat
    HAL_INS_PROBE_LIST;
#endif
}

bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
{

    if (!backend) {
        return false;
    }
    if (_backend_count == INS_MAX_BACKENDS) {
        AP_HAL::panic("Too many INS backends");
    }
    _backends[_backend_count++] = backend;
    return true;
}
    HAL_INS_PROBE_LIST宏定义在中，如下所示。
//build/Pixhawk1/hwdef.h
//不编译程序，没有这个文件
#define HAL_INS_PROBE1 ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this,hal.spi->get_device("mpu6000"),ROTATION_ROLL_180))
#define HAL_INS_PROBE2 ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,hal.spi->get_device("lsm9ds0_g"),hal.spi->get_device("lsm9ds0_am"),ROTATION_ROLL_180,ROTATION_ROLL_180_YAW_270,ROTATION_PITCH_180))
#define HAL_INS_PROBE_LIST HAL_INS_PROBE1;HAL_INS_PROBE2
    ADD_BACKEND()宏应传入一个AP_InertialSensor_Backend *类型的指针，也就是我们要添加的具体型号的INS设备对象的指针，probe函数用于new一个这样的对象，并返回指针。通过detect_backends函数我们便将每个INS设备作为一个具体的后端实现加入到_backends数组中，以便下一步依次启动。
for (uint8_t i = 0; i < _backend_count; i++)
{
   _backends->start();
}
    start函数内容较复杂，多是根据具体传感器型号进行初始化的内容，如设置量称，采样频率，低通滤波频率以及传感器的方向等这里不再叙述，其精简内容如下。
//libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
void AP_InertialSensor_Invensense::start()
{
//向前端注册
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev));
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev));
... ...     ... ..
// allocate fifo buffer
//数据缓冲区
_fifo_buffer = (uint8_t *)hal.util->malloc_type(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);

// start the timer process to read samples
//在SPI驱动线程里注册一个回调函数用来刷新传感器数据
_dev->register_periodic_callback(1000000UL / _backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
}
    这里主要分析一下register_periodic_callback函数，这个函数属于"_dev","_dev"即上述的"hal.spi->get_device("mpu6000")"或"hal.spi->get_device("lsm9ds0_g")"所返回的驱动实例。
//libraries/AP_HAL_ChibiOS/SPIDevice.cpp
AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
{
  return bus.register_periodic_callback(period_usec, cb, this);
}

//libraries/AP_HAL_ChibiOS/Device.cpp
AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device)
{
//如果这个驱动的线程还没有被创建，创建
if (!thread_started)
{
   thread_started = true;

   hal_device = _hal_device;
   // setup a name for the thread
   const uint8_t name_len = 7;
   char *name = (char *)malloc(name_len);
   switch (hal_device->bus_type()) {
   case AP_HAL::Device::BUS_TYPE_I2C:
   snprintf(name, name_len, "I2C:%u",
   hal_device->bus_num());
   break;

   case AP_HAL::Device::BUS_TYPE_SPI:
   snprintf(name, name_len, "SPI:%u",
   hal_device->bus_num());
   break;
   default:
   break;
        }
   //开辟线程
   thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(HAL_DEVICE_THREAD_STACK),
   name,
   thread_priority,           /* Initial priority.    */
   DeviceBus::bus_thread,    /* Thread function.     */
   this);                     /* Thread parameter.    */
   if (thread_ctx == nullptr) {
   AP_HAL::panic("Failed to create bus thread %s", name);
    }
}
//在线程内注册回调函数
DeviceBus::callback_info *callback = new DeviceBus::callback_info;
if (callback == nullptr) {
return nullptr;
    }
callback->cb = cb;
callback->period_usec = period_usec;
callback->next_usec = AP_HAL::micros64() + period_usec;

    // add to linked list of callbacks on thread
callback->next = callbacks;
callbacks = callback;

return callback;
}
    通过创建通信驱动模块(SPI或IIC)的线程后，再将传感器的数据刷新函数(AP_InertialSensor_Invensense::_poll_data)注册到这个线程内，这样便实现了传感器的初始化和后端的持续运行。
    到这里原始的传感器数据就源源不断的被读出来了，我们来看一下_poll_data函数是如何处理的。
//libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
/*Timer process to poll for new data from the Invensense. Called from bus thread with semaphore held*/
void AP_InertialSensor_Invensense::_poll_data()
{
_read_fifo();
}
_read_fifo()

//精简代码
void AP_InertialSensor_Invensense::_read_fifo()
{
uint8_t *rx = _fifo_buffer;
//检测MPU内数据包数量
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
   goto check_registers;
}

bytes_read = uint16_val(rx, 0);
n_samples = bytes_read / MPU_SAMPLE_SIZE;
//将n包数据循环读取到数组内
while (n_samples > 0)
{
  uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN);
  i_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)；
  _accumulate(rx, n)；
  n_samples -= n;
}
}
_accumulate()

bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)
{
//将n包数据循环处理
for (uint8_t i = 0; i < n_samples; i++)
{
    const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
    Vector3f accel, gyro;
    bool fsync_set = false;
    //加速度
    accel = Vector3f(int16_val(data, 1),
                     int16_val(data, 0),
                     -int16_val(data, 2));
    accel *= _accel_scale;
    //温度
    int16_t t2 = int16_val(data, 3);
    if (!_check_raw_temp(t2)) {
      debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
      _fifo_reset();
      return false;
    }
    float temp = t2 * temp_sensitivity + temp_zero;
    //角速度
    gyro = Vector3f(int16_val(data, 5),
                    int16_val(data, 4),
                    -int16_val(data, 6));
    gyro *= _gyro_scale;
    //加速度、角速度信息坐标系变换、数据处理
    _rotate_and_correct_accel(_accel_instance, accel);
    _rotate_and_correct_gyro(_gyro_instance, gyro);
    //通知新的加速度、角速度数据刷新。
    //对原始采样数据低通滤波。根据加速度计算速度增量。
    _notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
    _notify_new_gyro_raw_sample(_gyro_instance, gyro);

    _temp_filtered = _temp_filter.apply(temp);
  }
return true;
}
    至此，传感器的后端回调函数便完成了数据的读取，误差处理，坐标系变换，以及低通滤波和速度增量计算。
    但是，后端采集到的数据如何被前端使用呢，也就是说前后端是如何通信的呢，前后端同时访问这些数据的话，如何保证正确访问？
    前后端数据交换是通过几个变量完成的，后端将"_notify_new_xxx_raw_sample"函数处理好的数据放到前端的数组内，主线程中前端ins按一定频率update访问这个数组，这样便将数据传递了。
    下边对"_notify_new_xxx_raw_sample"函数和update函数分别分析说明。
//libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
//精简代码
void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
const Vector3f &accel,
uint64_t sample_us,
bool fsync_set)
{
float dt;
dt = 1.0f / _imu._accel_raw_sample_rates[instance];
{
  WITH_SEMAPHORE(_sem);
  // delta velocity
  _imu._delta_velocity_acc[instance] += accel * dt;
  _imu._delta_velocity_acc_dt[instance] += dt;

  _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
  _imu._new_accel_data[instance] = true;
}
}
    这段代码是对加速度数据的处理，根据原始的加速度信息计算速度增量，并给加速度低通滤波，最终通过标志位设为true通知前端数据刷新了。
//libraries/AP_InertialSensor/AP_InertialSensor.cpp
//前端update精简代码
/*update gyro and accel values from backends*/
void AP_InertialSensor::update(void)
{
wait_for_sample();//等待一定的时间间隔

for (uint8_t i=0; i<_backend_count; i++) {
   _backends->update();//调用后端刷新
}
}
    后端刷新代码如下
//libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
/*publish any pending data*/
bool AP_InertialSensor_Invensense::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);

_publish_temperature(_accel_instance, _temp_filtered);

return true;
}
    以update_accel为例说明
//libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
/* common accel update function for all backends*/
void AP_InertialSensor_Backend::update_accel(uint8_t instance)
{   
WITH_SEMAPHORE(_sem);

if (_imu._new_accel_data[instance]) {
   _publish_accel(instance, _imu._accel_filtered[instance]);
   _imu._new_accel_data[instance] = false;
  }
}
    通过上述数据刷新标志位判断是否发布加速度信息。
//精简代码
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel)
{
_imu._accel[instance] = accel;
_imu._accel_healthy[instance] = true;

    // publish delta velocity
_imu._delta_velocity[instance] = _imu._delta_velocity_acc[instance];
_imu._delta_velocity_dt[instance] = _imu._delta_velocity_acc_dt[instance];
_imu._delta_velocity_valid[instance] = true;
}
    这样便将传感器的数据存储到了_imu._accel和_imu._delta_velocity数组中了，完成了数据的传递，完成了前后端的通信。其他数据，如陀螺仪等都是相同的方式。
数据访问锁

    以上程序中定义有一句宏定义：

    WITH_SEMAPHORE(_sem);

    这个宏是定义了一个局部Semaphore，在其定义时开始锁定线程，跳出其作用域后自动解锁。

// semaphore for access to shared frontend data
HAL_Semaphore_Recursive _sem;

古风剑客

学习了，谢谢大佬[调皮]