Article Directory
foreword
APM 4.2.3
Take the serial port driver of the distance measuring sensor as an example to read.
Other sensor drivers are similar to it
. If there is any omission or error, please point out
一、AP_RangeFinder_TeraRanger_Serial. h
The ranging sensor drivers of all serial port protocols inherit fromAP_RangeFinder_Backend_Serial
class AP_RangeFinder_TeraRanger_Serial : public AP_RangeFinder_Backend_Serial
{
AP_RangeFinder_Backend_SerialIt is an abstract class, which provides the driving interface of different ranging sensors through pure virtual functions. The declaration of the class is as follows:
create is a static member function. This function creates an instance of the AP_RangeFinder_TeraRanger_Serial class and converts it into a pointer of the base class AP_RangeFinder_Backend_Serial and returns it. Through this function, the base class pointer can point to the subclass object and realize polymorphism
public:
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_TeraRanger_Serial(_state, _params);
}
Here, the base class constructor is inherited by using, so that the base class constructor can be directly used in the subclass
protected:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
Implement the interface of the parent class in the child class
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
In this file, the get_reading interface in the base class is implemented. In this interface, the business logic specific to TeraRanger is implemented, and the members of the UARTDriver class are called through the base class uart pointer to read the serial port.
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
3. AP_RangeFinder.cpp
This function implements the main logic driven by the ranging sensor, and the main functions are as follows:
init
Initialization function, this function runs when the system is initialized, as shown in the figure below:
This function mainly initializes the parameters and status of the sensor, and calls the detect_instance function to query the sensor interface. This function is explained below
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
The function of the detect_instance function is to call the corresponding subclass for different sensors.
serial_create_fn is a pointer to a function that returns a pointer to the base class AP_RangeFinder_Backend_Serial. This pointer points to a different subclass, and the corresponding subclass interface function can be called to achieve multiple state, take AP_RangeFinder_TeraRanger_Serial as an example, as shown in the figure below,
this function will call the _add_backend function to put the interface into a pointer array, so that it is convenient to call the corresponding interface in turn through the array
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
This function is to put the sensor interface found above into the pointer array drivers and call it in 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
The update function will call the update function to update the sensor data. update is also an interface. The TeraRanger sensor inherits from AP_RangeFinder_Backend_Serial, and its corresponding update function is implemented in 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
This is mainly to initialize the port and baud rate, as well as update the read data. The update function is called in AP_RangeFinder.cpp, and get_reading is called in update. Here get_reading is an interface, which is implemented in the second section AP_RangeFinder_TeraRanger_Serial Yes, the reading of the serial port sensor is completed here.
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);
}
}