mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
8e0124f383
this fixes the case where we have one rangefinder that can handle short range and another that is good for long range but no good for short range (quite common, eg radar and lidar) If possible we want to use the first rangefinder that is in range for the right orientation. If none are in range then use the first for the orientation
709 lines
24 KiB
C++
709 lines
24 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "RangeFinder.h"
|
|
#include "AP_RangeFinder_analog.h"
|
|
#include "AP_RangeFinder_PulsedLightLRF.h"
|
|
#include "AP_RangeFinder_MaxsonarI2CXL.h"
|
|
#include "AP_RangeFinder_MaxsonarSerialLV.h"
|
|
#include "AP_RangeFinder_BBB_PRU.h"
|
|
#include "AP_RangeFinder_LightWareI2C.h"
|
|
#include "AP_RangeFinder_LightWareSerial.h"
|
|
#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
|
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && \
|
|
defined(HAVE_LIBIIO)
|
|
#include "AP_RangeFinder_Bebop.h"
|
|
#endif
|
|
#include "AP_RangeFinder_MAVLink.h"
|
|
#include "AP_RangeFinder_LeddarOne.h"
|
|
#include "AP_RangeFinder_uLanding.h"
|
|
#include "AP_RangeFinder_TeraRangerI2C.h"
|
|
#include "AP_RangeFinder_VL53L0X.h"
|
|
#include "AP_RangeFinder_VL53L1X.h"
|
|
#include "AP_RangeFinder_NMEA.h"
|
|
#include "AP_RangeFinder_Wasp.h"
|
|
#include "AP_RangeFinder_Benewake.h"
|
|
#include "AP_RangeFinder_Benewake_TFMiniPlus.h"
|
|
#include "AP_RangeFinder_PWM.h"
|
|
#include "AP_RangeFinder_BLPing.h"
|
|
#include "AP_RangeFinder_UAVCAN.h"
|
|
#include "AP_RangeFinder_Lanbao.h"
|
|
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
|
#include <AP_Logger/AP_Logger.h>
|
|
#include <AP_SerialManager/AP_SerialManager.h>
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
// table of user settable parameters
|
|
const AP_Param::GroupInfo RangeFinder::var_info[] = {
|
|
|
|
// @Group: 1_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[0], "1_", 25, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 1_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]),
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 1
|
|
// @Group: 2_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[1], "2_", 27, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 2_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]),
|
|
#endif
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 2
|
|
// @Group: 3_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[2], "3_", 29, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 3_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]),
|
|
#endif
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 3
|
|
// @Group: 4_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[3], "4_", 31, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 4_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]),
|
|
#endif
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 4
|
|
// @Group: 5_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[4], "5_", 33, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 5_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]),
|
|
#endif
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 5
|
|
// @Group: 6_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[5], "6_", 35, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 6_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]),
|
|
#endif
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 6
|
|
// @Group: 7_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[6], "7_", 37, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 7_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]),
|
|
#endif
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 7
|
|
// @Group: 8_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[7], "8_", 39, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 8_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]),
|
|
#endif
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 8
|
|
// @Group: 9_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[8], "9_", 41, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: 9_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]),
|
|
#endif
|
|
|
|
#if RANGEFINDER_MAX_INSTANCES > 9
|
|
// @Group: A_
|
|
// @Path: AP_RangeFinder_Params.cpp
|
|
AP_SUBGROUPINFO(params[9], "A_", 43, RangeFinder, AP_RangeFinder_Params),
|
|
|
|
// @Group: A_
|
|
// @Path: AP_RangeFinder_Wasp.cpp
|
|
AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]),
|
|
#endif
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
const AP_Param::GroupInfo *RangeFinder::backend_var_info[RANGEFINDER_MAX_INSTANCES];
|
|
|
|
RangeFinder::RangeFinder()
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
if (_singleton != nullptr) {
|
|
AP_HAL::panic("Rangefinder must be singleton");
|
|
}
|
|
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
_singleton = this;
|
|
}
|
|
|
|
void RangeFinder::convert_params(void) {
|
|
if (params[0].type.configured_in_storage()) {
|
|
// _params[0]._type will always be configured in storage after conversion is done the first time
|
|
return;
|
|
}
|
|
|
|
struct ConversionTable {
|
|
uint8_t old_element;
|
|
uint8_t new_index;
|
|
uint8_t instance;
|
|
};
|
|
|
|
const struct ConversionTable conversionTable[] = {
|
|
// rangefinder 1
|
|
{0, 0, 0}, //0, TYPE 1
|
|
{1, 1, 0}, //1, PIN 1
|
|
{2, 2, 0}, //2, SCALING 1
|
|
{3, 3, 0}, //3, OFFSET 1
|
|
{4, 4, 0}, //4, FUNCTION 1
|
|
{5, 5, 0}, //5, MIN_CM 1
|
|
{6, 6, 0}, //6, MAX_CM 1
|
|
{7, 7, 0}, //7, STOP_PIN 1
|
|
{9, 8, 0}, //9, RMETRIC 1
|
|
{10, 9, 0}, //10, PWRRNG 1 (previously existed only once for all sensors)
|
|
{11, 10, 0}, //11, GNDCLEAR 1
|
|
{23, 11, 0}, //23, ADDR 1
|
|
{49, 12, 0}, //49, POS 1
|
|
{53, 13, 0}, //53, ORIENT 1
|
|
|
|
// rangefinder 2
|
|
{12, 0, 1}, //12, TYPE 2
|
|
{13, 1, 1}, //13, PIN 2
|
|
{14, 2, 1}, //14, SCALING 2
|
|
{15, 3, 1}, //15, OFFSET 2
|
|
{16, 4, 1}, //16, FUNCTION 2
|
|
{17, 5, 1}, //17, MIN_CM 2
|
|
{18, 6, 1}, //18, MAX_CM 2
|
|
{19, 7, 1}, //19, STOP_PIN 2
|
|
{21, 8, 1}, //21, RMETRIC 2
|
|
{10, 9, 1}, //10, PWRRNG 1 (previously existed only once for all sensors)
|
|
{22, 10, 1}, //22, GNDCLEAR 2
|
|
{24, 11, 1}, //24, ADDR 2
|
|
{50, 12, 1}, //50, POS 2
|
|
{54, 13, 1}, //54, ORIENT 2
|
|
};
|
|
|
|
char param_name[17] = {0};
|
|
AP_Param::ConversionInfo info;
|
|
info.new_name = param_name;
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
|
info.old_key = 71;
|
|
#elif APM_BUILD_TYPE(APM_BUILD_ArduCopter)
|
|
info.old_key = 53;
|
|
#elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
|
|
info.old_key = 35;
|
|
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
|
|
info.old_key = 197;
|
|
#else
|
|
params[0].type.save(true);
|
|
return; // no conversion is supported on this platform
|
|
#endif
|
|
|
|
for (uint8_t i = 0; i < ARRAY_SIZE(conversionTable); i++) {
|
|
uint8_t param_instance = conversionTable[i].instance + 1;
|
|
uint8_t destination_index = conversionTable[i].new_index;
|
|
|
|
info.old_group_element = conversionTable[i].old_element;
|
|
info.type = (ap_var_type)AP_RangeFinder_Params::var_info[destination_index].type;
|
|
|
|
hal.util->snprintf(param_name, sizeof(param_name), "RNGFND%X_%s", param_instance, AP_RangeFinder_Params::var_info[destination_index].name);
|
|
param_name[sizeof(param_name)-1] = '\0';
|
|
|
|
AP_Param::convert_old_parameter(&info, 1.0f, 0);
|
|
}
|
|
|
|
// force _params[0]._type into storage to flag that conversion has been done
|
|
params[0].type.save(true);
|
|
}
|
|
|
|
/*
|
|
initialise the RangeFinder class. We do detection of attached range
|
|
finders here. For now we won't allow for hot-plugging of
|
|
rangefinders.
|
|
*/
|
|
void RangeFinder::init(enum Rotation orientation_default)
|
|
{
|
|
if (num_instances != 0) {
|
|
// init called a 2nd time?
|
|
return;
|
|
}
|
|
|
|
convert_params();
|
|
|
|
// 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
|
|
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)
|
|
num_instances = i+1;
|
|
}
|
|
|
|
// initialise status
|
|
state[i].status = RangeFinder_NotConnected;
|
|
state[i].range_valid_count = 0;
|
|
}
|
|
}
|
|
|
|
/*
|
|
update RangeFinder state for all instances. This should be called at
|
|
around 10Hz by main loop
|
|
*/
|
|
void RangeFinder::update(void)
|
|
{
|
|
for (uint8_t i=0; i<num_instances; i++) {
|
|
if (drivers[i] != nullptr) {
|
|
if (params[i].type == RangeFinder_TYPE_NONE) {
|
|
// allow user to disable a rangefinder at runtime
|
|
state[i].status = RangeFinder_NotConnected;
|
|
state[i].range_valid_count = 0;
|
|
continue;
|
|
}
|
|
drivers[i]->update();
|
|
}
|
|
}
|
|
#ifndef HAL_BUILD_AP_PERIPH
|
|
Log_RFND();
|
|
#endif
|
|
}
|
|
|
|
bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
|
|
{
|
|
if (!backend) {
|
|
return false;
|
|
}
|
|
if (num_instances == RANGEFINDER_MAX_INSTANCES) {
|
|
AP_HAL::panic("Too many RANGERS backends");
|
|
}
|
|
|
|
drivers[num_instances++] = backend;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
detect if an instance of a rangefinder is connected.
|
|
*/
|
|
void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
|
|
{
|
|
enum RangeFinder_Type _type = (enum RangeFinder_Type)params[instance].type.get();
|
|
switch (_type) {
|
|
case RangeFinder_TYPE_PLI2C:
|
|
case RangeFinder_TYPE_PLI2CV3:
|
|
case RangeFinder_TYPE_PLI2CV3HP:
|
|
FOREACH_I2C(i) {
|
|
if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], params[instance], _type))) {
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_MBI2C:
|
|
FOREACH_I2C(i) {
|
|
if (_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance],
|
|
hal.i2c_mgr->get_device(i, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)))) {
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_LWI2C:
|
|
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)));
|
|
#else
|
|
FOREACH_I2C(i) {
|
|
if (_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
|
|
hal.i2c_mgr->get_device(i, params[instance].address)))) {
|
|
break;
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_TRI2C:
|
|
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)))) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_VL53L0X:
|
|
FOREACH_I2C(i) {
|
|
if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
|
|
hal.i2c_mgr->get_device(i, params[instance].address)))) {
|
|
break;
|
|
}
|
|
if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance],
|
|
hal.i2c_mgr->get_device(i, params[instance].address)))) {
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_BenewakeTFminiPlus:
|
|
FOREACH_I2C(i) {
|
|
if (_add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect(state[instance], params[instance],
|
|
hal.i2c_mgr->get_device(i, params[instance].address)))) {
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
|
case RangeFinder_TYPE_PX4_PWM:
|
|
#ifndef HAL_BUILD_AP_PERIPH
|
|
// to ease moving from PX4 to ChibiOS we'll lie a little about
|
|
// the backend driver...
|
|
if (AP_RangeFinder_PWM::detect()) {
|
|
drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height);
|
|
}
|
|
#endif
|
|
break;
|
|
#endif
|
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
|
|
case RangeFinder_TYPE_BBB_PRU:
|
|
if (AP_RangeFinder_BBB_PRU::detect()) {
|
|
drivers[instance] = new AP_RangeFinder_BBB_PRU(state[instance], params[instance]);
|
|
}
|
|
break;
|
|
#endif
|
|
case RangeFinder_TYPE_LWSER:
|
|
if (AP_RangeFinder_LightWareSerial::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], params[instance], serial_instance++);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_LEDDARONE:
|
|
if (AP_RangeFinder_LeddarOne::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], params[instance], serial_instance++);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_ULANDING:
|
|
if (AP_RangeFinder_uLanding::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_uLanding(state[instance], params[instance], serial_instance++);
|
|
}
|
|
break;
|
|
#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
|
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && defined(HAVE_LIBIIO)
|
|
case RangeFinder_TYPE_BEBOP:
|
|
if (AP_RangeFinder_Bebop::detect()) {
|
|
drivers[instance] = new AP_RangeFinder_Bebop(state[instance], params[instance]);
|
|
}
|
|
break;
|
|
#endif
|
|
case RangeFinder_TYPE_MAVLink:
|
|
#ifndef HAL_BUILD_AP_PERIPH
|
|
if (AP_RangeFinder_MAVLink::detect()) {
|
|
drivers[instance] = new AP_RangeFinder_MAVLink(state[instance], params[instance]);
|
|
}
|
|
#endif
|
|
break;
|
|
case RangeFinder_TYPE_MBSER:
|
|
if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance], serial_instance++);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_ANALOG:
|
|
#ifndef HAL_BUILD_AP_PERIPH
|
|
// note that analog will always come back as present if the pin is valid
|
|
if (AP_RangeFinder_analog::detect(params[instance])) {
|
|
drivers[instance] = new AP_RangeFinder_analog(state[instance], params[instance]);
|
|
}
|
|
#endif
|
|
break;
|
|
case RangeFinder_TYPE_NMEA:
|
|
if (AP_RangeFinder_NMEA::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_NMEA(state[instance], params[instance], serial_instance++);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_WASP:
|
|
if (AP_RangeFinder_Wasp::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_Wasp(state[instance], params[instance], serial_instance++);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_BenewakeTF02:
|
|
if (AP_RangeFinder_Benewake::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_BenewakeTFmini:
|
|
if (AP_RangeFinder_Benewake::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_BenewakeTF03:
|
|
if (AP_RangeFinder_Benewake::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF03);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_PWM:
|
|
#ifndef HAL_BUILD_AP_PERIPH
|
|
if (AP_RangeFinder_PWM::detect()) {
|
|
drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height);
|
|
}
|
|
#endif
|
|
break;
|
|
case RangeFinder_TYPE_BLPing:
|
|
if (AP_RangeFinder_BLPing::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_BLPing(state[instance], params[instance], serial_instance++);
|
|
}
|
|
break;
|
|
case RangeFinder_TYPE_Lanbao:
|
|
if (AP_RangeFinder_Lanbao::detect(serial_instance)) {
|
|
drivers[instance] = new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++);
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
// 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]);
|
|
}
|
|
}
|
|
|
|
AP_RangeFinder_Backend *RangeFinder::get_backend(uint8_t id) const {
|
|
if (id >= num_instances) {
|
|
return nullptr;
|
|
}
|
|
if (drivers[id] != nullptr) {
|
|
if (drivers[id]->type() == RangeFinder_TYPE_NONE) {
|
|
// pretend it isn't here; disabled at runtime?
|
|
return nullptr;
|
|
}
|
|
}
|
|
return drivers[id];
|
|
};
|
|
|
|
RangeFinder::RangeFinder_Status RangeFinder::status_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return RangeFinder_NotConnected;
|
|
}
|
|
return backend->status();
|
|
}
|
|
|
|
void RangeFinder::handle_msg(const mavlink_message_t &msg)
|
|
{
|
|
uint8_t i;
|
|
for (i=0; i<num_instances; i++) {
|
|
if ((drivers[i] != nullptr) && (params[i].type != RangeFinder_TYPE_NONE)) {
|
|
drivers[i]->handle_msg(msg);
|
|
}
|
|
}
|
|
}
|
|
|
|
// return true if we have a range finder with the specified orientation
|
|
bool RangeFinder::has_orientation(enum Rotation orientation) const
|
|
{
|
|
return (find_instance(orientation) != nullptr);
|
|
}
|
|
|
|
// find first range finder instance with the specified orientation
|
|
AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) const
|
|
{
|
|
// first try for a rangefinder that is in range
|
|
for (uint8_t i=0; i<num_instances; i++) {
|
|
AP_RangeFinder_Backend *backend = get_backend(i);
|
|
if (backend != nullptr &&
|
|
backend->orientation() == orientation &&
|
|
backend->status() == RangeFinder_Good) {
|
|
return backend;
|
|
}
|
|
}
|
|
// if none in range then return first with correct orientation
|
|
for (uint8_t i=0; i<num_instances; i++) {
|
|
AP_RangeFinder_Backend *backend = get_backend(i);
|
|
if (backend != nullptr &&
|
|
backend->orientation() == orientation) {
|
|
return backend;
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return 0;
|
|
}
|
|
return backend->distance_cm();
|
|
}
|
|
|
|
uint16_t RangeFinder::voltage_mv_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return 0;
|
|
}
|
|
return backend->voltage_mv();
|
|
}
|
|
|
|
int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return 0;
|
|
}
|
|
return backend->max_distance_cm();
|
|
}
|
|
|
|
int16_t RangeFinder::min_distance_cm_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return 0;
|
|
}
|
|
return backend->min_distance_cm();
|
|
}
|
|
|
|
int16_t RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return 0;
|
|
}
|
|
return backend->ground_clearance_cm();
|
|
}
|
|
|
|
bool RangeFinder::has_data_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return false;
|
|
}
|
|
return backend->has_data();
|
|
}
|
|
|
|
uint8_t RangeFinder::range_valid_count_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return 0;
|
|
}
|
|
return backend->range_valid_count();
|
|
}
|
|
|
|
const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return pos_offset_zero;
|
|
}
|
|
return backend->get_pos_offset();
|
|
}
|
|
|
|
uint32_t RangeFinder::last_reading_ms(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return 0;
|
|
}
|
|
return backend->last_reading_ms();
|
|
}
|
|
|
|
MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotation orientation) const
|
|
{
|
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
|
if (backend == nullptr) {
|
|
return MAV_DISTANCE_SENSOR_UNKNOWN;
|
|
}
|
|
return backend->get_mav_distance_sensor_type();
|
|
}
|
|
|
|
// Write an RFND (rangefinder) packet
|
|
void RangeFinder::Log_RFND()
|
|
{
|
|
if (_log_rfnd_bit == uint32_t(-1)) {
|
|
return;
|
|
}
|
|
|
|
AP_Logger &logger = AP::logger();
|
|
if (!logger.should_log(_log_rfnd_bit)) {
|
|
return;
|
|
}
|
|
|
|
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
|
|
const AP_RangeFinder_Backend *s = get_backend(i);
|
|
if (s == nullptr) {
|
|
continue;
|
|
}
|
|
|
|
const struct log_RFND pkt = {
|
|
LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
|
|
time_us : AP_HAL::micros64(),
|
|
instance : i,
|
|
dist : s->distance_cm(),
|
|
status : (uint8_t)s->status(),
|
|
orient : s->orientation(),
|
|
};
|
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
|
}
|
|
}
|
|
|
|
bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const
|
|
{
|
|
for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
|
|
if ((params[i].type != RangeFinder_TYPE_NONE) && (drivers[i] == nullptr)) {
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
RangeFinder *RangeFinder::_singleton;
|
|
|
|
namespace AP {
|
|
|
|
RangeFinder *rangefinder()
|
|
{
|
|
return RangeFinder::get_singleton();
|
|
}
|
|
|
|
}
|