設備代碼層次結構
Ardupilot設備驅動代碼的層次結構采用 前端實現 和 后端實現 分割，前端庫主要供機器代碼層調用，后端庫主要供前端調用。這里前端可以理解為應用層，后端理解為驅動層，前端調用后端代碼，實際是驅動層提供接口供應用層使用。
前端調用后端代碼之前，系統會通過自動檢測設備或者通過用戶配置的參數創建並且啟動一個或者多個后端對象。用戶自定義參數(_TYPE)，例如RNGFND_TYPE。每個后端對象都會保存在前端創建的指針數組中( _drivers[])。
設備驅動代碼被調用方式
圖中左邊的后端設備驅動代碼運行於后台線程中，主要實現從外部設備讀取原始數據，轉化為標准單位，並且將處理后的數據存儲在緩沖區中。具體的飛行控制器代碼通過調用前端代碼獲取最新的設備數據，並在主線程中周期處理運行(400HZ for copter)。例如從傳感器的前端代碼中讀取加速度計、陀螺儀數據等。
其中，為了不阻礙主線程的運行，IIC和SPI通信在后台線程中運行。但主線程中可以調用USART接口函數，因為為底層的串行驅動程序本身在后台收集數據保存在一個緩沖區中。
飛行控制上層代碼調用設備前段代碼示例
sensors.cpp文件中包含有調用設備驅動前端代碼，例如飛控控制以20HZ的頻率調用read_rangefinder()函數而讀取高度數據，而該函數內部則調用了rangefinder.update() 和rangefinder.set_estimated_terrain_height()前端代碼來獲取數據。摘取代碼如下：
/*
read the rangefinder and update height estimate
*/
void Plane::read_rangefinder(void)
{
/* notify the rangefinder of our approximate altitude above ground to allow it to power on*/
/* during low-altitude flight when configured to power down during higher-altitude flight*/
float height;
#if AP_TERRAIN_AVAILABLE
if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(height, true)) {
rangefinder.set_estimated_terrain_height(height);
} else
#endif
{
/* use the best available alt estimate via baro above home*/
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
/* ensure the rangefinder is powered-on when land alt is higher than home altitude.
This is done using the target alt which we know is below us and we are sinking to it*/
height = height_above_target();
} else {
/* otherwise just use the best available baro estimate above home.*/
height = relative_altitude;
}
rangefinder.set_estimated_terrain_height(height); //設置地形估計高度
}
rangefinder.update(); //通過傳感器更新高度數據
if ((rangefinder.num_sensors() > 0) && should_log(MASK_LOG_SONAR)) {
Log_Write_Sonar();
}
rangefinder_height_update();
}
以下代碼為rangefinder.update()函數內部實現
/*
update the state of the sensor by usart
*/
void AP_RangeFinder_LightWareSerial::update(void)
{
//獲取緩沖區中獲取的原始數據，並且將處理后的數據保存至distance_cm中，數據為true，否則為false
if (get_reading(state.distance_cm)) {
// update range_valid state based on distance measured
last_reading_ms = AP_HAL::millis(); //獲取當前系統運行時間
update_status(); /*判斷distance_cm數據情況，高於最大測量范圍或者小於最小測量范圍或者數據正常*/
} else if (AP_HAL::millis() - last_reading_ms > 200) { /* 超過200ms緩沖沒有數據 */
set_status(RangeFinder::RangeFinder_NoData);
}
}
串口設備后端實現示例
此處以獲取LightWare數據為例，首先需通過serial_manager類和用戶設置的參數獲取串口設備對象實例。代碼如下：
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state, AP_SerialManager &serial_manager) : AP_RangeFinder_Backend(_state)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0));
}
}
前端代碼在讀取串口數據之前，需每次調用update()方法獲取串口接受緩沖區中的數據，update方法中則調用的get_reading()方法將數據讀取的內存中進行數據處理。其中關於update的代碼可見之前的rangefinder.update()代碼的實現，另外get_reading()代碼如下：
// read - return last value measured by sensor
bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
}
// read any available lines from the lidar
float sum = 0;
uint16_t count = 0;
int16_t nbytes = uart->available(); //檢測串口接收緩沖區中的數據個數
while (nbytes-- > 0) { //將緩沖區的數據讀出，可能會讀到多組數據
char c = uart->read(); //獲取一個字符
if (c == '\r') { //一組數據以'\r'為結尾
linebuf[linebuf_len] = 0;
sum += (float)atof(linebuf); //將浮點字符串轉換成字符串
count++;
linebuf_len = 0;
} else if (isdigit(c) || c == '.') { //判斷數據是否有效
linebuf[linebuf_len++] = c;
if (linebuf_len == sizeof(linebuf)) {
// too long, discard the line
linebuf_len = 0;
}
}
}
// we need to write a byte to prompt another reading
uart->write('d');
if (count == 0) {
return false; //無數據返回false
}
reading_cm = 100 * sum / count; //單位換算成cm，並且求多組數據的平均值
return true; //有數據返回true
}
因系統中每個串口都有自己接收緩沖區，所以主線程中可以直接調用get_reading()方法，而不影響其性能。而IIC和SPI通信則需要通過另外的機制來獲取數據。
IIC后端代碼實例
前端代碼通過指定IIC設備的地址而對IIC實例對象進行初始化，初始化代碼位於RangeFinder.cpp文件中的RangeFinder::detect_instance(uint8_t instance)函數中：
case RangeFinder_TYPE_LWI2C:
if (state[instance].address) {
#ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance],
hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, state[instance].address)));
#else
if (!_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance],
hal.i2c_mgr->get_device(1, state[instance].address)))) {
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance],
hal.i2c_mgr->get_device(0, state[instance].address)));
}
#endif
}
break;
其中代碼
hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, state[instance].address)
通過指定IIC地址在總線上得到對應設備。指定設備之后，則可以通過調用相應的后端代碼來初始化該設備與讀取數據。代碼如下：
void AP_RangeFinder_LightWareI2C::init()
{
// call timer() at 20Hz 以20HZ的頻率執行定時器回調函數
_dev->register_periodic_callback(50000,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::timer, void));
}
// read - return last value measured by sensor
bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
{
be16_t val;
if (state.address == 0) {
return false;
}
// read the high and low byte distance registers
bool ret = _dev->read((uint8_t *) &val, sizeof(val));
if (ret) {
// combine results into distance
reading_cm = be16toh(val);
}
return ret;
}
而定時回調函數中則調用了get_reading()方法獲取IIC設備的數據。
SPI后端代碼實例
以MPU9250 IMU后端代碼介紹SPI總線后端代碼的編寫。獲取SPI設備對象的初始化代碼類似於IIC，代碼位於AP_InertialSensor.cpp文件AP_InertialSensor::detect_backends(void)函數中。
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); //獲取SPI設備
此外，在后台線程中start()方法會自動調用對SPI總線上對應設備(此處為MPU9250)進行初始化和配置。程序中使用信號量區別SPI總線上的不同設備。
其中，_read_sample()方法被注冊以1000HZ的頻率被調用。__block_read()方法則主要從傳感器寄存器中獲取數據供上層代碼處理。
注意
如果添加新的設備驅動程序代碼，則在代碼中絕對不能有任何的等待或者線程休眠代碼，因為這樣會影響其他線程所使用的總線。
如果想將新的驅動代碼加入的工程中，則必須在make.inc和wscript文件中編寫相應的工程代碼，這兩個文件位於對應的飛行器代碼目錄下(ArduPlane、ArduCopter...)。這樣新編寫的驅動才會參與工程代碼的編譯，最后一同生成可執行的二進制文件。后續可將該文件燒寫至飛控處理器中運行。