Ardupilot -- APM源码笔记三（重制）~ 传感器驱动程序

Ardupilot支持来自多个产商的传感器，比如链接库中可以明显看到的测距仪。这个章节将大概讲解传感器驱动程序的编写并集成到源码。比较偏底层的知识，涉及控制板的数据传输接口跟底层实现，刚开始学习源码的读者可以忽略此章节，避免不必要的烦恼。

---

支持的协议

I2C, SPI, UART(Serial) 和 CANBUS 协议都是被支持的。 如果需要编写一个新的驱动，需要根据传感器的数据传输来确定使用哪个协议。

I2C

• 一个主设备，多个从设备
• 一个相对简单的协议，适用于短距离通讯（即小于一米）
• 运行速率在100k hz或400k hz，但是相比其他协议的数据传输速率相对较低
• 需要两根线，SDA（数据）和SCL（时钟）

SPI

• 一个主设备，一个从设备
• 20M hz+的速度，相对I2C来说是非常快的
• 只能在短距离使用（10厘米）
• 需要四根线，分别是CS（片选）、MOSI（主发从收）、MISO（从发主收）、CLK（时钟）

Serial / UART

• 一主一从
• 字符数据的通讯协议（对I2C、SPI）有着更远距离的优势（1米）
• 相对较快的传输 57Kbps ~ 1.5Mbps
• 需要四根线，分别是VCC（电源）、GND（地）、RX（收） 、TX（发）

CAN bus with UAVCAN

• multimaster bus, any node can initiate transmittion of data when they need to
• packet based protocol for very long distances
• high speed, typically 1 Mb (however only 50% of the bus bitrate can really be used without major collisions)
• at least 3 pins required (GND, CAN HI, CAN LO). Optionally VCC can be used to power nodes
• point-to-point topology. Star or stubs topolgy is not advised
• termination is required at each end of the bus

---

前后端区分

传感器驱动程序架构中一个重要的概念是前后端分离

• Ardupilot代码只能调用库（即传感器驱动程序）的前端
• 在前端启动前创建一个或多个后端基于传感器的自动检测（即检测一个已知的I2C地址的响应）或通过使用用户定义的_TYPE参数（如RNGFND_TYPE, RNGFND_TYPE2），通常把后端的指针保留在前端一个数组_drivers[]中
• 用户总是通过前端来设置参数

驱动代码的运行

上图显示的Ardupilot架构放大图，左上角的蓝色框说明传感器驱动程序的后端是在后台线程中运行，从传感器收集原始数据,转化为标准单位,然后保留在缓冲区内
源码的主线程会定期轮循（像多旋翼是每400hz轮循一次），通过访问传感器前端代码来获得最新数据。举个栗子，为了取得姿态估算的数据， AHRS/EKF（航向姿态参考系统）会从传感器驱动程序前端抽取加速度、陀螺仪、罗盘信息等数据
图示为后端传感器数据获取的简单描述，它们需要在后台线程运行，与传感器频繁交流，不会影响到主循环线程的执行，但当驱动程序在主线程调用串行接口却是安全的，因为底层的串行驱动程序本身在后台收集数据,包括一个缓冲区。

前段代码示例

下面示例代码展示了多旋翼拉取测距仪（声呐、超声波等）获取数据，调度程序以20hz调用read_rangefinder()函数，你可以看到下面为这个函数的截图，位于sensors.cpp文件中，通过调用rangefinder.update( )和rangefinder.distance_cm( )函数来调用驱动程序的前端

下面是测距仪前端的更新函数，这使得我们可以在主线程中做一些其他处理，每个后端更新函数都被依次调用

/*
  update RangeFinder state for all instances. This should be called at
  around 10Hz by main loop
 */
void RangeFinder::update(void)
{
    for (uint8_t i=0; iif (drivers[i] != NULL) {
            if (_type[i] == RangeFinder_TYPE_NONE) {
                // allow user to disable a rangefinder at runtime
                state[i].status = RangeFinder_NotConnected;
                state[i].range_valid_count = 0;
                continue;
            }
            drivers[i]->update();
            update_pre_arm_check(i);
        }
    }

    // work out primary instance - first sensor returning good data
    for (int8_t i=num_instances-1; i>=0; i--) {
        if (drivers[i] != NULL && (state[i].status == RangeFinder_Good)) {
            primary_instance = i;
        }
    }
}

串行接口后端示例

接下来的LightWare后端更新函数所使用的是串行协议。根据wiki所述测距仪可以连接上任何飞行控制器的串行端口，但用户必须指定对应的串行端口及波特率，通过设置参数SERIALX_BAUD和SERIALX_PROTOCOL

在LightWare启动串行驱动程序代码，它将检测到用户想通过serial_manager类寻找上述参数设置

每次后端update( )被调用时若检查到来自传感器的新字符将会调用get_reading函数去解码它们
如上所述，因为串行协议创建自己的数据缓冲，在主线程中处理任何从传感器拿到的数据（可见get_reading函数），所以不会在I2C或SPI的驱动程序中看到处理数据的回调函数

/* 
   update the state of the sensor
*/
void AP_RangeFinder_LightWareSerial::update(void)
{
    if (get_reading(state.distance_cm)) {
        // update range_valid state based on distance measured
        last_reading_ms = AP_HAL::millis();
        update_status();
    } else if (AP_HAL::millis() - last_reading_ms > 200) {
        set_status(RangeFinder::RangeFinder_NoData);
    }
}

I2C后端示例

例子展示了Lightware I2C的后端驱动程序，前端I2C总线初始化同时将其传递到后端。

后端初始化同时配置了一个20hz的定时器，利用定时器从传感器读取字节再转换为厘米的距离单位

SPI后端示例

这里将展示部分MPU9250 IMU（惯性测量系统）的后端程序，包括了陀螺仪、加速度、罗盘等传感器数据，前端SPI总线初始化同时将其同步到后端

调用start（）函数来初始化跟配置传感器，通过信号量机制来确保不干扰到同一总线上的其他SPI设备
以1000hz来跑信号量状态_read_sample（）函数确保信号量的实时获取，但注意在_read_sample（）函数中不需要有获取信号量跟释放信号量的操作，因为这个函数是个周期性的回调函数
_block_read（）函数展示程序是怎么从传感器的寄存器获得数据的