Directorio de artículos
prefacio
APM 4.2.3
Tome el controlador del puerto serie del sensor de medición de distancia como ejemplo para leer.
Otros controladores de sensor son similares
. Si hay alguna omisión o error, indíquelo
Ejemplo: AP_RangeFinder_TeraRanger_Serial.h
Los controladores de sensor de alcance de todos los protocolos de puerto serie heredan deAP_RangeFinder_Backend_Serial
class AP_RangeFinder_TeraRanger_Serial : public AP_RangeFinder_Backend_Serial
{
AP_RangeFinder_Backend_SerialEs una clase abstracta, que proporciona la interfaz de conducción de diferentes sensores de rango a través de funciones virtuales puras.La declaración de la clase es la siguiente:
create es una función miembro estática. Esta función crea una instancia de la clase AP_RangeFinder_TeraRanger_Serial y la convierte en un puntero de la clase base AP_RangeFinder_Backend_Serial y lo devuelve. A través de esta función, el puntero de la clase base puede apuntar al objeto de la subclase y realizar el polimorfismo.
public:
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_TeraRanger_Serial(_state, _params);
}
Aquí, el constructor de la clase base se hereda usando, de modo que el constructor de la clase base se puede usar directamente en la subclase
protected:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
Implementar la interfaz de la clase padre en la clase hijo
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
Ejemplo: AP_RangeFinder_TeraRanger_Serial.cpp
En este archivo, se implementa la interfaz get_reading en la clase base.En esta interfaz, se implementa la lógica empresarial específica de TeraRanger y se llama a los miembros de la clase UARTDriver a través del puntero uart de la clase base para leer el puerto serie.
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
Esta función implementa la lógica principal impulsada por el sensor de rango, y las funciones principales son las siguientes:
en eso
Función de inicialización, esta función se ejecuta cuando se inicializa el sistema, como se muestra en la siguiente figura:
Esta función inicializa principalmente los parámetros y el estado del sensor, y llama a la función detect_instance para consultar la interfaz del sensor. Esta función se explica a continuación.
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;
}
}
detectar_instancia
La función de la función detect_instance es llamar a la subclase correspondiente para diferentes sensores.
serial_create_fn es un puntero a una función que devuelve un puntero a la clase base AP_RangeFinder_Backend_Serial. Este puntero apunta a una subclase diferente, y se puede llamar a la función de interfaz de subclase correspondiente para lograr múltiples estados, tome AP_RangeFinder_TeraRanger_Serial como ejemplo, como se muestra en la figura a continuación,
esta función llamará a la función _add_backend para colocar la interfaz en una matriz de punteros, de modo que sea conveniente llamar a la interfaz correspondiente a su vez a través de la matriz
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
Esta función es para poner la interfaz del sensor que se encuentra arriba en los controladores de la matriz de punteros y llamarla en la actualización
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;
}
actualizar
La función de actualización llamará a la función de actualización para actualizar los datos del sensor. La actualización también es una interfaz. El sensor TeraRanger hereda de AP_RangeFinder_Backend_Serial, y su función de actualización correspondiente se implementa en 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
}
Otro ejemplo: AP_RangeFinder_Backend_Serial.cpp
Esto es principalmente para inicializar el puerto y la velocidad en baudios, así como actualizar los datos de lectura. La función de actualización se llama en AP_RangeFinder.cpp, y get_reading se llama en update. Aquí get_reading es una interfaz, que se implementa en la segunda sección AP_RangeFinder_TeraRanger_Serial Sí, aquí se completa la lectura del sensor del puerto serie.
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);
}
}