文章目錄

• 前言

• 一、AP_RangeFinder_TeraRanger_Serial.h

• 二、AP_RangeFinder_TeraRanger_Serial.cpp

• 三、AP_RangeFinder.cpp

• init

• detect_instance

• _add_backend

• update

• 四、 AP_RangeFinder_Backend_Serial.cpp

前言

APM 4.2.3
以測(cè)距傳感器的串口驅(qū)動(dòng)為例進(jìn)行閱讀
其他的傳感驅(qū)動(dòng)都與之類似
如果疏漏或謬誤，懇請(qǐng)指出

一、AP_RangeFinder_TeraRanger_Serial.h

所有串口協(xié)議的測(cè)距傳感器驅(qū)動(dòng)都繼承自AP_RangeFinder_Backend_Serial

class AP_RangeFinder_TeraRanger_Serial : public AP_RangeFinder_Backend_Serial{

AP_RangeFinder_Backend_Serial是一個(gè)抽象類，里面通過(guò)純虛函數(shù)提供了不同測(cè)距傳感器的驅(qū)動(dòng)接口，類的聲明如下圖：

create是一個(gè)靜態(tài)成員函數(shù)，該函數(shù)創(chuàng)建一個(gè)AP_RangeFinder_TeraRanger_Serial類的實(shí)例并轉(zhuǎn)化成基類AP_RangeFinder_Backend_Serial 的指針然后返回，通過(guò)這個(gè)函數(shù)可以實(shí)現(xiàn)基類指針指向子類對(duì)象，實(shí)現(xiàn)多態(tài)

public: ? ?static AP_RangeFinder_Backend_Serial *create( ? ? ? ?RangeFinder::RangeFinder_State &_state, ? ? ? ?AP_RangeFinder_Params &_params) { ? ? ? ?return new AP_RangeFinder_TeraRanger_Serial(_state, _params); ? ?}

這里通過(guò)using繼承基類構(gòu)造函數(shù)，從而可以在子類中直接使用基類構(gòu)造函數(shù)

protected: ? ?using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;

在子類實(shí)現(xiàn)父類的接口

? MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { ? ? ? ?return MAV_DISTANCE_SENSOR_LASER; ? ?}private: ? ?// get a reading ? ?// distance returned in reading_m ? ?bool get_reading(float &reading_m) override; ? ?uint8_t linebuf[10]; ? ?uint8_t linebuf_len;};#endif ?// AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED

二、AP_RangeFinder_TeraRanger_Serial.cpp

這個(gè)文件里面就是實(shí)現(xiàn)了基類中的get_reading接口，在該接口實(shí)現(xiàn)具體針對(duì)TeraRanger的業(yè)務(wù)邏輯，也是通過(guò)基類uart指針調(diào)用UARTDriver類的成員來(lái)對(duì)串口進(jìn)行讀取等操作

extern const AP_HAL::HAL& hal;#define FRAME_HEADER 0x54#define FRAME_LENGTH 5#define DIST_MAX_CM 3000#define OUT_OF_RANGE_ADD_CM 1000#define STATUS_MASK 0x1F#define DISTANCE_ERROR 0x0001// format of serial packets received from rangefinder//// Data Bit ? ? ? ? ? ? Definition ? ? ?Description// ------------------------------------------------// byte 0 ? ? ? ? ? ? ? Frame header ? ?0x54// byte 1 ? ? ? ? ? ? ? DIST_H ? ? ? ? ?Distance (in mm) high 8 bits// byte 2 ? ? ? ? ? ? ? DIST_L ? ? ? ? ?Distance (in mm) low 8 bits// byte 3 ? ? ? ? ? ? ? STATUS ? ? ? ? ?Status,Strengh,OverTemp// byte 4 ? ? ? ? ? ? ? CRC8 ? ? ? ? ? ?packet CRC// distance returned in reading_m, set to true if sensor reports a good readingbool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m){ ? ?if (uart == nullptr) { ? ? ? ?return false; ? ?} ? ?float sum_mm = 0; ? ?uint16_t count = 0; ? ?uint16_t bad_read = 0; ? ?// read any available lines from the lidar ? ?int16_t nbytes = uart->available(); ? ?while (nbytes-- > 0) { ? ? ? ?int16_t r = uart->read(); ? ? ? ?if (r < 0) { ? ? ? ? ? ?continue; ? ? ? ?} ? ? ? ?uint8_t c = (uint8_t)r; ? ? ? ?// if buffer is empty and this byte is 0x57, add to buffer ? ? ? ?if (linebuf_len == 0) { ? ? ? ? ? ?if (c == FRAME_HEADER) { ? ? ? ? ? ? ? ?linebuf[linebuf_len++] = c; ? ? ? ? ? ?} ? ? ? ?// buffer is not empty, add byte to buffer ? ? ? ?} else { ? ? ? ? ? ?// add character to buffer ? ? ? ? ? ?linebuf[linebuf_len++] = c; ? ? ? ? ? ?// if buffer now has 5 items try to decode it ? ? ? ? ? ?if (linebuf_len == FRAME_LENGTH) { ? ? ? ? ? ? ? ?// calculate CRC8 (tbd) ? ? ? ? ? ? ? ?uint8_t crc = 0; ? ? ? ? ? ? ? ?crc =crc_crc8(linebuf,FRAME_LENGTH-1); ? ? ? ? ? ? ? ?// if crc matches, extract contents ? ? ? ? ? ? ? ?if (crc == linebuf[FRAME_LENGTH-1]) { ? ? ? ? ? ? ? ? ? ?// calculate distance ? ? ? ? ? ? ? ? ? ?uint16_t dist = ((uint16_t)linebuf[1] << 8) | linebuf[2]; ? ? ? ? ? ? ? ? ? ?if (dist >= DIST_MAX_CM *10) { ? ? ? ? ? ? ? ? ? ? ? ?// this reading is out of range and a bad read ? ? ? ? ? ? ? ? ? ? ? ?bad_read++; ? ? ? ? ? ? ? ? ? ?} else { ? ? ? ? ? ? ? ? ? ? ? ?// check if reading is good, no errors, no overtemp, reading is not the special case of 1mm ? ? ? ? ? ? ? ? ? ? ? ?if ((STATUS_MASK & linebuf[3]) == 0 && (dist != DISTANCE_ERROR)) { ? ? ? ? ? ? ? ? ? ? ? ? ? ?// add distance to sum ? ? ? ? ? ? ? ? ? ? ? ? ? ?sum_mm += dist; ? ? ? ? ? ? ? ? ? ? ? ? ? ?count++; ? ? ? ? ? ? ? ? ? ? ? ?} else { ? ? ? ? ? ? ? ? ? ? ? ? ? ?// this reading is bad ? ? ? ? ? ? ? ? ? ? ? ? ? ?bad_read++; ? ? ? ? ? ? ? ? ? ? ? ?} ? ? ? ? ? ? ? ? ? ?} ? ? ? ? ? ? ? ?} ? ? ? ? ? ? ? ?// clear buffer ? ? ? ? ? ? ? ?linebuf_len = 0; ? ? ? ? ? ?} ? ? ? ?} ? ?} ? ?if (count > 0) { ? ? ? ?// return average distance of readings since last update ? ? ? ?reading_m = (sum_mm * 0.001f) / count; ? ? ? ?return true; ? ?} ? ?if (bad_read > 0) { ? ? ? ?// if a bad read has occurred this update overwrite return with larger of ? ? ? ?// driver defined maximum range for the model and user defined max range + 1m ? ? ? ?reading_m = MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM) * 0.01f; ? ? ? ?return true; ? ?} ? ?// no readings so return false ? ?return false;}#endif // AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED

三、AP_RangeFinder.cpp

這個(gè)函數(shù)實(shí)現(xiàn)了測(cè)距傳感器驅(qū)動(dòng)的主要邏輯，里面主要的函數(shù)如下：

init

初始化函數(shù)，這個(gè)函數(shù)在系統(tǒng)初始化時(shí)運(yùn)行，如下圖：

這個(gè)函數(shù)里面主要是初始化了傳感器的參數(shù)和狀態(tài)，調(diào)用了detect_instance函數(shù)對(duì)傳感器接口進(jìn)行查詢，這個(gè)函數(shù)在下面講解

void RangeFinder::init(enum Rotation orientation_default){ ? ?if (init_done) { ? ? ? ?// init called a 2nd time? ? ? ? ?return; ? ?} ? ?init_done = true; ? ?// set orientation defaults ? ?for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) { ? ? ? ?params[i].orientation.set_default(orientation_default); ? ?} ? ?for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) { ? ? ? ?// serial_instance will be increased inside detect_instance ? ? ? ?// if a serial driver is loaded for this instance ? ? ? ?WITH_SEMAPHORE(detect_sem); ? ? ? ?detect_instance(i, serial_instance); ? ? ? ?if (drivers[i] != nullptr) { ? ? ? ? ? ?// we loaded a driver for this instance, so it must be ? ? ? ? ? ?// present (although it may not be healthy). We use MAX() ? ? ? ? ? ?// here as a UAVCAN rangefinder may already have been ? ? ? ? ? ?// found ? ? ? ? ? ?num_instances = MAX(num_instances, i+1); ? ? ? ?} ? ? ? ?// initialise status ? ? ? ?state[i].status = Status::NotConnected; ? ? ? ?state[i].range_valid_count = 0; ? ?}}

detect_instance

detect_instance函數(shù)的作用就是針對(duì)不同的傳感器，調(diào)用相應(yīng)的子類
serial_create_fn是一個(gè)指向返回基類AP_RangeFinder_Backend_Serial 指針的函數(shù)的指針，這個(gè)指針指向不同的子類，就可以調(diào)用相應(yīng)的子類接口函數(shù)，從而實(shí)現(xiàn)多態(tài)，以AP_RangeFinder_TeraRanger_Serial為例，如下圖，

這個(gè)函數(shù)會(huì)調(diào)用_add_backend函數(shù)將接口放到一個(gè)指針數(shù)組中，方便通過(guò)數(shù)組輪流調(diào)用相應(yīng)的接口

void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance){#if AP_RANGEFINDER_ENABLED ? ?AP_RangeFinder_Backend_Serial *(*serial_create_fn)(RangeFinder::RangeFinder_State&, AP_RangeFinder_Params&) = nullptr; ? ?const Type _type = (Type)params[instance].type.get(); ? ?switch (_type) { ? ?case Type::PLI2C: ? ?case Type::PLI2CV3: ? ?case Type::PLI2CV3HP:#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED ? ? ? ?FOREACH_I2C(i) { ? ? ? ? ? ?if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], params[instance], _type), ? ? ? ? ? ? ? ? ? ? ? ? ? ? instance)) { ? ? ? ? ? ? ? ?break; ? ? ? ? ? ?} ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::MBI2C: {#if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED ? ? ? ?uint8_t addr = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR; ? ? ? ?if (params[instance].address != 0) { ? ? ? ? ? ?addr = params[instance].address; ? ? ? ?} ? ? ? ?FOREACH_I2C(i) { ? ? ? ? ? ?if (_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance], ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?hal.i2c_mgr->get_device(i, addr)), ? ? ? ? ? ? ? ? ? ? ? ? ? ? instance)) { ? ? ? ? ? ? ? ?break; ? ? ? ? ? ?} ? ? ? ?} ? ? ? ?break;#endif ? ?} ? ?case Type::LWI2C:#if AP_RANGEFINDER_LWI2C_ENABLED ? ? ? ?if (params[instance].address) { ? ? ? ? ? ?// the LW20 needs a long time to boot up, so we delay 1.5s here ? ? ? ? ? ?if (!hal.util->was_watchdog_armed()) { ? ? ? ? ? ? ? ?hal.scheduler->delay(1500); ? ? ? ? ? ?}#ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS ? ? ? ? ? ?_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance], ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, params[instance].address)), ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? instance);#else ? ? ? ? ? ?FOREACH_I2C(i) { ? ? ? ? ? ? ? ?if (_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance], ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? hal.i2c_mgr->get_device(i, params[instance].address)), ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? instance)) { ? ? ? ? ? ? ? ? ? ?break; ? ? ? ? ? ? ? ?} ? ? ? ? ? ?}#endif ? ? ? ?}#endif ?// AP_RANGEFINDER_LWI2C_ENABLED ? ? ? ?break; ? ?case Type::TRI2C:#if AP_RANGEFINDER_TRI2C_ENABLED ? ? ? ?if (params[instance].address) { ? ? ? ? ? ?FOREACH_I2C(i) { ? ? ? ? ? ? ? ?if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance], ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?hal.i2c_mgr->get_device(i, params[instance].address)), ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? instance)) { ? ? ? ? ? ? ? ? ? ?break; ? ? ? ? ? ? ? ?} ? ? ? ? ? ?} ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::VL53L0X: ? ?case Type::VL53L1X_Short: ? ? ? ? ? ?FOREACH_I2C(i) {#if AP_RANGEFINDER_VL53L0X_ENABLED ? ? ? ? ? ? ? ?if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance], ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?hal.i2c_mgr->get_device(i, params[instance].address)), ? ? ? ? ? ? ? ? ? ? ? ?instance)) { ? ? ? ? ? ? ? ? ? ?break; ? ? ? ? ? ? ? ?}#endif#if AP_RANGEFINDER_VL53L1X_ENABLED ? ? ? ? ? ? ? ?if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance], ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?hal.i2c_mgr->get_device(i, params[instance].address), ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?_type == Type::VL53L1X_Short ? ?AP_RangeFinder_VL53L1X::DistanceMode::Short : ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?AP_RangeFinder_VL53L1X::DistanceMode::Long), ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? instance)) { ? ? ? ? ? ? ? ? ? ?break; ? ? ? ? ? ? ? ?}#endif ? ? ? ? ? ?} ? ? ? ?break; ? ?case Type::BenewakeTFminiPlus: {#if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED ? ? ? ?uint8_t addr = TFMINIPLUS_ADDR_DEFAULT; ? ? ? ?if (params[instance].address != 0) { ? ? ? ? ? ?addr = params[instance].address; ? ? ? ?} ? ? ? ?FOREACH_I2C(i) { ? ? ? ? ? ?if (_add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect(state[instance], params[instance], ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?hal.i2c_mgr->get_device(i, addr)), ? ? ? ? ? ? ? ? ? ?instance)) { ? ? ? ? ? ? ? ?break; ? ? ? ? ? ?} ? ? ? ?} ? ? ? ?break;#endif ? ?} ? ?case Type::PX4_PWM:#if AP_RANGEFINDER_PWM_ENABLED ? ? ? ?// to ease moving from PX4 to ChibiOS we'll lie a little about ? ? ? ?// the backend driver... ? ? ? ?if (AP_RangeFinder_PWM::detect()) { ? ? ? ? ? ?_add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance); ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::BBB_PRU:#if AP_RANGEFINDER_BBB_PRU_ENABLED ? ? ? ?if (AP_RangeFinder_BBB_PRU::detect()) { ? ? ? ? ? ?_add_backend(new AP_RangeFinder_BBB_PRU(state[instance], params[instance]), instance); ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::LWSER:#if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_LightWareSerial::create;#endif ? ? ? ?break; ? ?case Type::LEDDARONE:#if AP_RANGEFINDER_LEDDARONE_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_LeddarOne::create;#endif ? ? ? ?break; ? ?case Type::USD1_Serial:#if AP_RANGEFINDER_USD1_SERIAL_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_USD1_Serial::create;#endif ? ? ? ?break; ? ?case Type::BEBOP:#if AP_RANGEFINDER_BEBOP_ENABLED ? ? ? ?if (AP_RangeFinder_Bebop::detect()) { ? ? ? ? ? ?_add_backend(new AP_RangeFinder_Bebop(state[instance], params[instance]), instance); ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::MAVLink:#if AP_RANGEFINDER_MAVLINK_ENABLED ? ? ? ?if (AP_RangeFinder_MAVLink::detect()) { ? ? ? ? ? ?_add_backend(new AP_RangeFinder_MAVLink(state[instance], params[instance]), instance); ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::MBSER:#if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_MaxsonarSerialLV::create;#endif ? ? ? ?break; ? ?case Type::ANALOG:#if AP_RANGEFINDER_ANALOG_ENABLED ? ? ? ?// note that analog will always come back as present if the pin is valid ? ? ? ?if (AP_RangeFinder_analog::detect(params[instance])) { ? ? ? ? ? ?_add_backend(new AP_RangeFinder_analog(state[instance], params[instance]), instance); ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::HC_SR04:#if AP_RANGEFINDER_HC_SR04_ENABLED ? ? ? ?// note that this will always come back as present if the pin is valid ? ? ? ?if (AP_RangeFinder_HC_SR04::detect(params[instance])) { ? ? ? ? ? ?_add_backend(new AP_RangeFinder_HC_SR04(state[instance], params[instance]), instance); ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::NMEA:#if AP_RANGEFINDER_NMEA_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_NMEA::create;#endif ? ? ? ?break; ? ?case Type::WASP:#if AP_RANGEFINDER_WASP_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_Wasp::create;#endif ? ? ? ?break; ? ?case Type::BenewakeTF02:#if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_Benewake_TF02::create;#endif ? ? ? ?break; ? ?case Type::BenewakeTFmini:#if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_Benewake_TFMini::create;#endif ? ? ? ?break; ? ?case Type::BenewakeTF03:#if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_Benewake_TF03::create;#endif ? ? ? ?break; ? ?case Type::TeraRanger_Serial:#if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_TeraRanger_Serial::create;#endif ? ? ? ?break; ? ?case Type::PWM:#if AP_RANGEFINDER_PWM_ENABLED ? ? ? ?if (AP_RangeFinder_PWM::detect()) { ? ? ? ? ? ?_add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance); ? ? ? ?}#endif ? ? ? ?break; ? ?case Type::BLPing:#if AP_RANGEFINDER_BLPING_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_BLPing::create;#endif ? ? ? ?break; ? ?case Type::Lanbao:#if AP_RANGEFINDER_LANBAO_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_Lanbao::create;#endif ? ? ? ?break; ? ?case Type::LeddarVu8_Serial:#if AP_RANGEFINDER_LEDDARVU8_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_LeddarVu8::create;#endif ? ? ? ?break; ? ?case Type::UAVCAN:#if AP_RANGEFINDER_UAVCAN_ENABLED ? ? ? ?/* ? ? ? ? ?the UAVCAN driver gets created when we first receive a ? ? ? ? ?measurement. We take the instance slot now, even if we don't ? ? ? ? ?yet have the driver ? ? ? ? */ ? ? ? ?num_instances = MAX(num_instances, instance+1);#endif ? ? ? ?break; ? ?case Type::GYUS42v2:#if AP_RANGEFINDER_GYUS42V2_ENABLED ? ? ? ?serial_create_fn = AP_RangeFinder_GYUS42v2::create;#endif ? ? ? ?break; ? ?case Type::SIM:#if AP_RANGEFINDER_SIM_ENABLED ? ? ? ?_add_backend(new AP_RangeFinder_SITL(state[instance], params[instance], instance), instance);#endif ? ? ? ?break; ? ?case Type::MSP:#if HAL_MSP_RANGEFINDER_ENABLED ? ? ? ?if (AP_RangeFinder_MSP::detect()) { ? ? ? ? ? ?_add_backend(new AP_RangeFinder_MSP(state[instance], params[instance]), instance); ? ? ? ?}#endif // HAL_MSP_RANGEFINDER_ENABLED ? ? ? ?break; ? ?case Type::USD1_CAN:#if AP_RANGEFINDER_USD1_CAN_ENABLED ? ? ? ?_add_backend(new AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance);#endif ? ? ? ?break; ? ?case Type::Benewake_CAN:#if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED ? ? ? ?_add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance); ? ? ? ?break;#endif ? ?case Type::NONE: ? ? ? ?break; ? ?} ? ?if (serial_create_fn != nullptr) { ? ? ? ?if (AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance)) { ? ? ? ? ? ?auto *b = serial_create_fn(state[instance], params[instance]); ? ? ? ? ? ?if (b != nullptr) { ? ? ? ? ? ? ? ?_add_backend(b, instance, serial_instance++); ? ? ? ? ? ?} ? ? ? ?} ? ?} ? ?// if the backend has some local parameters then make those available in the tree ? ?if (drivers[instance] && state[instance].var_info) { ? ? ? ?backend_var_info[instance] = state[instance].var_info; ? ? ? ?AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]); ? ? ? ?// param count could have changed ? ? ? ?AP_Param::invalidate_count(); ? ?}#endif //AP_RANGEFINDER_ENABLED}

_add_backend

這個(gè)函數(shù)就是把上面查找到的傳感器接口放入指針數(shù)組drivers中，在update中調(diào)用

bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance, uint8_t serial_instance){ ? ?if (!backend) { ? ? ? ?return false; ? ?} ? ?if (instance >= RANGEFINDER_MAX_INSTANCES) { ? ? ? ?AP_HAL::panic("Too many RANGERS backends"); ? ?} ? ?if (drivers[instance] != nullptr) { ? ? ? ?// we've allocated the same instance twice ? ? ? ?INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); ? ?} ? ?backend->init_serial(serial_instance); ? ?drivers[instance] = backend; ? ?num_instances = MAX(num_instances, instance+1); ? ?return true;}

update

update函數(shù)中會(huì)調(diào)用update函數(shù)對(duì)傳感器數(shù)據(jù)進(jìn)行更新，update也是一個(gè)接口，TeraRanger傳感器繼承自AP_RangeFinder_Backend_Serial，其對(duì)應(yīng)的update函數(shù)在AP_RangeFinder_Backend_Serial.cpp中實(shí)現(xiàn)

void RangeFinder::update(void){ ? ?for (uint8_t i=0; i<num_instances; i++) { ? ? ? ?if (drivers[i] != nullptr) { ? ? ? ? ? ?if ((Type)params[i].type.get() == Type::NONE) { ? ? ? ? ? ? ? ?// allow user to disable a rangefinder at runtime ? ? ? ? ? ? ? ?state[i].status = Status::NotConnected; ? ? ? ? ? ? ? ?state[i].range_valid_count = 0; ? ? ? ? ? ? ? ?continue; ? ? ? ? ? ?} ? ? ? ? ? ?drivers[i]->update(); ? ? ? ?} ? ?}#if HAL_LOGGING_ENABLED ? ?Log_RFND();#endif}

四、 AP_RangeFinder_Backend_Serial.cpp

這里主要是初始化端口和波特率，還有更新讀取的數(shù)據(jù)，update函數(shù)就是在AP_RangeFinder.cpp中調(diào)用的，在update中會(huì)調(diào)用get_reading，這里的get_reading是一個(gè)接口，就是第二節(jié)AP_RangeFinder_TeraRanger_Serial中實(shí)現(xiàn)的，到這里就完成了串口傳感器的讀取。

void AP_RangeFinder_Backend_Serial::init_serial(uint8_t serial_instance){ ? ?uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); ? ?if (uart != nullptr) { ? ? ? ?uart->begin(initial_baudrate(serial_instance), rx_bufsize(), tx_bufsize()); ? ?}}uint32_t AP_RangeFinder_Backend_Serial::initial_baudrate(const uint8_t serial_instance) const{ ? ?return AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);}/* ? update the state of the sensor */void AP_RangeFinder_Backend_Serial::update(void){ ? ?if (get_reading(state.distance_m)) { ? ? ? ?// update range_valid state based on distance measured ? ? ? ?state.last_reading_ms = AP_HAL::millis(); ? ? ? ?update_status(); ? ?} else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) { ? ? ? ?set_status(RangeFinder::Status::NoData); ? ?}}