Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use RangeFinder backend defines better #25412

Merged
merged 1 commit into from
Nov 1, 2023
Merged

Use RangeFinder backend defines better #25412

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
160 changes: 84 additions & 76 deletions libraries/AP_RangeFinder/AP_RangeFinder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@
defined(HAVE_LIBIIO)
#include "AP_RangeFinder_Bebop.h"
#endif
#include "AP_RangeFinder_Backend.h"
#include "AP_RangeFinder_Backend_Serial.h"
#include "AP_RangeFinder_MAVLink.h"
#include "AP_RangeFinder_LeddarOne.h"
#include "AP_RangeFinder_USD1_Serial.h"
Expand Down Expand Up @@ -271,20 +273,20 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)

const Type _type = (Type)params[instance].type.get();
switch (_type) {
#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
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: {
#endif
#if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED
case Type::MBI2C: {
uint8_t addr = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR;
if (params[instance].address != 0) {
addr = params[instance].address;
Expand All @@ -297,10 +299,10 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
}
}
break;
#endif
}
case Type::LWI2C:
#endif
#if AP_RANGEFINDER_LWI2C_ENABLED
case Type::LWI2C:
if (params[instance].address) {
// the LW20 needs a long time to boot up, so we delay 1.5s here
#ifndef HAL_BUILD_AP_PERIPH
Expand All @@ -322,10 +324,10 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
}
#endif
}
#endif // AP_RANGEFINDER_LWI2C_ENABLED
break;
case Type::TRI2C:
#endif // AP_RANGEFINDER_LWI2C_ENABLED
#if AP_RANGEFINDER_TRI2C_ENABLED
case Type::TRI2C:
if (params[instance].address) {
FOREACH_I2C(i) {
if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance],
Expand All @@ -335,8 +337,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
}
}
}
#endif
break;
#endif
case Type::VL53L0X:
case Type::VL53L1X_Short:
FOREACH_I2C(i) {
Expand All @@ -358,8 +360,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
#endif
}
break;
case Type::BenewakeTFminiPlus: {
#if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED
case Type::BenewakeTFminiPlus: {
uint8_t addr = TFMINIPLUS_ADDR_DEFAULT;
if (params[instance].address != 0) {
addr = params[instance].address;
Expand All @@ -372,193 +374,193 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
}
}
break;
#endif
}
case Type::PX4_PWM:
#endif
#if AP_RANGEFINDER_PWM_ENABLED
case Type::PX4_PWM:
// 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:
#endif
#if AP_RANGEFINDER_BBB_PRU_ENABLED
case Type::BBB_PRU:
if (AP_RangeFinder_BBB_PRU::detect()) {
_add_backend(new AP_RangeFinder_BBB_PRU(state[instance], params[instance]), instance);
}
#endif
break;
case Type::LWSER:
#endif
#if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED
case Type::LWSER:
serial_create_fn = AP_RangeFinder_LightWareSerial::create;
#endif
break;
case Type::LEDDARONE:
#endif
#if AP_RANGEFINDER_LEDDARONE_ENABLED
case Type::LEDDARONE:
serial_create_fn = AP_RangeFinder_LeddarOne::create;
#endif
break;
case Type::USD1_Serial:
#endif
#if AP_RANGEFINDER_USD1_SERIAL_ENABLED
case Type::USD1_Serial:
serial_create_fn = AP_RangeFinder_USD1_Serial::create;
#endif
break;
case Type::BEBOP:
#endif
#if AP_RANGEFINDER_BEBOP_ENABLED
case Type::BEBOP:
if (AP_RangeFinder_Bebop::detect()) {
_add_backend(new AP_RangeFinder_Bebop(state[instance], params[instance]), instance);
}
#endif
break;
case Type::MAVLink:
#endif
#if AP_RANGEFINDER_MAVLINK_ENABLED
case Type::MAVLink:
if (AP_RangeFinder_MAVLink::detect()) {
_add_backend(new AP_RangeFinder_MAVLink(state[instance], params[instance]), instance);
}
#endif
break;
case Type::MBSER:
#endif
#if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED
case Type::MBSER:
serial_create_fn = AP_RangeFinder_MaxsonarSerialLV::create;
#endif
break;
case Type::ANALOG:
#endif
#if AP_RANGEFINDER_ANALOG_ENABLED
case Type::ANALOG:
// 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:
#endif
#if AP_RANGEFINDER_HC_SR04_ENABLED
case Type::HC_SR04:
// 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:
#endif
#if AP_RANGEFINDER_NMEA_ENABLED
case Type::NMEA:
serial_create_fn = AP_RangeFinder_NMEA::create;
#endif
break;
case Type::WASP:
#endif
#if AP_RANGEFINDER_WASP_ENABLED
case Type::WASP:
serial_create_fn = AP_RangeFinder_Wasp::create;
#endif
break;
case Type::BenewakeTF02:
#endif
#if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED
case Type::BenewakeTF02:
serial_create_fn = AP_RangeFinder_Benewake_TF02::create;
#endif
break;
case Type::BenewakeTFmini:
#endif
#if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED
case Type::BenewakeTFmini:
serial_create_fn = AP_RangeFinder_Benewake_TFMini::create;
#endif
break;
case Type::BenewakeTF03:
#endif
#if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED
case Type::BenewakeTF03:
serial_create_fn = AP_RangeFinder_Benewake_TF03::create;
#endif
break;
case Type::TeraRanger_Serial:
#endif
#if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
case Type::TeraRanger_Serial:
serial_create_fn = AP_RangeFinder_TeraRanger_Serial::create;
#endif
break;
case Type::PWM:
#endif
#if AP_RANGEFINDER_PWM_ENABLED
case Type::PWM:
if (AP_RangeFinder_PWM::detect()) {
_add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance);
}
#endif
break;
case Type::BLPing:
#endif
#if AP_RANGEFINDER_BLPING_ENABLED
case Type::BLPing:
serial_create_fn = AP_RangeFinder_BLPing::create;
#endif
break;
case Type::Lanbao:
#endif
#if AP_RANGEFINDER_LANBAO_ENABLED
case Type::Lanbao:
serial_create_fn = AP_RangeFinder_Lanbao::create;
#endif
break;
case Type::LeddarVu8_Serial:
#endif
#if AP_RANGEFINDER_LEDDARVU8_ENABLED
case Type::LeddarVu8_Serial:
serial_create_fn = AP_RangeFinder_LeddarVu8::create;
#endif
break;
#endif

case Type::UAVCAN:
#if AP_RANGEFINDER_DRONECAN_ENABLED
case Type::UAVCAN:
/*
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;
#endif

case Type::GYUS42v2:
#if AP_RANGEFINDER_GYUS42V2_ENABLED
case Type::GYUS42v2:
serial_create_fn = AP_RangeFinder_GYUS42v2::create;
#endif
break;
#endif

case Type::SIM:
#if AP_RANGEFINDER_SIM_ENABLED
case Type::SIM:
_add_backend(new AP_RangeFinder_SITL(state[instance], params[instance], instance), instance);
#endif
break;
#endif

case Type::MSP:
#if HAL_MSP_RANGEFINDER_ENABLED
case Type::MSP:
if (AP_RangeFinder_MSP::detect()) {
_add_backend(new AP_RangeFinder_MSP(state[instance], params[instance]), instance);
}
#endif // HAL_MSP_RANGEFINDER_ENABLED
break;
#endif // HAL_MSP_RANGEFINDER_ENABLED

case Type::USD1_CAN:
#if AP_RANGEFINDER_USD1_CAN_ENABLED
case Type::USD1_CAN:
_add_backend(new AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance);
#endif
break;
case Type::Benewake_CAN:
#endif
#if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
case Type::Benewake_CAN:
_add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance);
#endif
break;
#endif

case Type::Lua_Scripting:
#if AP_RANGEFINDER_LUA_ENABLED
case Type::Lua_Scripting:
_add_backend(new AP_RangeFinder_Lua(state[instance], params[instance]), instance);
#endif
break;
#endif

case Type::NoopLoop_P:
#if AP_RANGEFINDER_NOOPLOOP_ENABLED
case Type::NoopLoop_P:
serial_create_fn = AP_RangeFinder_NoopLoop::create;
#endif
break;
#endif

case Type::TOFSenseP_CAN:
#if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
case Type::TOFSenseP_CAN:
_add_backend(new AP_RangeFinder_TOFSenseP_CAN(state[instance], params[instance]), instance);
#endif
break;
case Type::NRA24_CAN:
#endif
#if AP_RANGEFINDER_NRA24_CAN_ENABLED
case Type::NRA24_CAN:
_add_backend(new AP_RangeFinder_NRA24_CAN(state[instance], params[instance]), instance);
#endif
break;
case Type::TOFSenseF_I2C: {
#endif
#if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
case Type::TOFSenseF_I2C: {
uint8_t addr = TOFSENSEP_I2C_DEFAULT_ADDR;
if (params[instance].address != 0) {
addr = params[instance].address;
Expand All @@ -571,9 +573,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
}
}
break;
#endif
}

#endif
case Type::NONE:
break;
}
Expand Down Expand Up @@ -816,19 +817,26 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le

// backend-specific checks. This might end up drivers[i]->arming_checks(...).
switch (drivers[i]->allocated_type()) {
#if AP_RANGEFINDER_PWM_ENABLED || AP_RANGEFINDER_ANALOG_ENABLED
#if AP_RANGEFINDER_ANALOG_ENABLED || AP_RANGEFINDER_PWM_ENABLED
#if AP_RANGEFINDER_ANALOG_ENABLED
case Type::ANALOG:
#endif
#if AP_RANGEFINDER_PWM_ENABLED
case Type::PX4_PWM:
case Type::PWM: {
case Type::PWM:
#endif
{
// ensure pin is configured
if (params[i].pin == -1) {
hal.util->snprintf(failure_msg, failure_msg_len, "RNGFND%u_PIN not set", unsigned(i + 1));
return false;
}
#if AP_RANGEFINDER_ANALOG_ENABLED
if (drivers[i]->allocated_type() == Type::ANALOG) {
// Analog backend does not use GPIO pin
break;
}
#endif

// ensure that the pin we're configured to use is available
if (!hal.gpio->valid_pin(params[i].pin)) {
Expand All @@ -842,7 +850,7 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le
}
break;
}
#endif
#endif // AP_RANGEFINDER_ANALOG_ENABLED || AP_RANGEFINDER_PWM_ENABLED

#if AP_RANGEFINDER_NRA24_CAN_ENABLED
case Type::NRA24_CAN: {
Expand Down
Loading