APM代码阅读（一）：串口驱动
文章目录
前言
一、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
以测距传感器的串口驱动为例进行阅读
其他的传感驱动都与之类似
如果疏漏或谬误，恳请指出
一、AP_RangeFinder_TeraRanger_Serial.h
所有串口协议的测距传感器驱动都继承自AP_RangeFinder_Backend_Serial
class AP_RangeFinder_TeraRanger_Serial : public AP_RangeFinder_Backend_Serial{
AP_RangeFinder_Backend_Serial是一个抽象类，里面通过纯虚函数提供了不同测距传感器的驱动接口，类的声明如下图：
create是一个静态成员函数，该函数创建一个AP_RangeFinder_TeraRanger_Serial类的实例并转化成基类AP_RangeFinder_Backend_Serial 的指针然后返回，通过这个函数可以实现基类指针指向子类对象，实现多态
public: static AP_RangeFinder_Backend_Serial *create( RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) { return new AP_RangeFinder_TeraRanger_Serial(_state, _params); }
这里通过using继承基类构造函数，从而可以在子类中直接使用基类构造函数
protected: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
在子类实现父类的接口
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
这个文件里面就是实现了基类中的get_reading接口，在该接口实现具体针对TeraRanger的业务逻辑，也是通过基类uart指针调用UARTDriver类的成员来对串口进行读取等操作
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
这个函数实现了测距传感器驱动的主要逻辑，里面主要的函数如下：
init
初始化函数，这个函数在系统初始化时运行，如下图：
这个函数里面主要是初始化了传感器的参数和状态，调用了detect_instance函数对传感器接口进行查询，这个函数在下面讲解
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函数的作用就是针对不同的传感器，调用相应的子类
serial_create_fn是一个指向返回基类AP_RangeFinder_Backend_Serial 指针的函数的指针，这个指针指向不同的子类，就可以调用相应的子类接口函数，从而实现多态，以AP_RangeFinder_TeraRanger_Serial为例，如下图，
这个函数会调用_add_backend函数将接口放到一个指针数组中，方便通过数组轮流调用相应的接口
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
这个函数就是把上面查找到的传感器接口放入指针数组drivers中，在update中调用
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函数中会调用update函数对传感器数据进行更新，update也是一个接口，TeraRanger传感器继承自AP_RangeFinder_Backend_Serial，其对应的update函数在AP_RangeFinder_Backend_Serial.cpp中实现
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
这里主要是初始化端口和波特率，还有更新读取的数据，update函数就是在AP_RangeFinder.cpp中调用的，在update中会调用get_reading，这里的get_reading是一个接口，就是第二节AP_RangeFinder_TeraRanger_Serial中实现的，到这里就完成了串口传感器的读取。
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); }}
