文章目录
- 前言
- 一、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 reading
bool 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);
}
}