Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: fixup detection of serial drivers

This commit is contained in:
Randy Mackay 2022-07-12 14:29:35 +09:00 committed by Andrew Tridgell
parent 826cf558eb
commit 965cdd7e72
5 changed files with 98 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

167
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -109,16 +109,92 @@ void AP_Proximity::init()
// init called a 2nd time?
return;
}
for (uint8_t i=0; i<PROXIMITY_MAX_INSTANCES; i++) {
detect_instance(i);
if (drivers[i] != nullptr) {
// instantiate backends
uint8_t serial_instance = 0;
for (uint8_t instance=0; instance<PROXIMITY_MAX_INSTANCES; instance++) {
switch (get_type(instance)) {
case Type::None:
break;
case Type::RPLidarA2:
if (AP_Proximity_RPLidarA2::detect(serial_instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance], serial_instance);
serial_instance++;
}
break;
case Type::MAV:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_MAV(*this, state[instance], params[instance]);
break;
case Type::TRTOWER:
if (AP_Proximity_TeraRangerTower::detect(serial_instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance], serial_instance);
serial_instance++;
}
break;
case Type::TRTOWEREVO:
if (AP_Proximity_TeraRangerTowerEvo::detect(serial_instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance], params[instance], serial_instance);
serial_instance++;
}
break;
case Type::RangeFinder:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance], params[instance]);
break;
case Type::SF40C:
if (AP_Proximity_LightWareSF40C::detect(serial_instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance], serial_instance);
serial_instance++;
}
break;
case Type::SF45B:
if (AP_Proximity_LightWareSF45B::detect(serial_instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance], params[instance], serial_instance);
serial_instance++;
}
break;
case Type::CYGBOT_D1:
#if AP_PROXIMITY_CYGBOT_ENABLED
if (AP_Proximity_Cygbot_D1::detect(serial_instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance], serial_instance);
serial_instance++;
}
# endif
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Type::SITL:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_SITL(*this, state[instance], params[instance]);
break;
case Type::AirSimSITL:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]);
break;
#endif
} // switch (get_type(instance))
if (drivers[instance] != nullptr) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i+1;
num_instances = instance+1;
}
// initialise status
state[i].status = Status::NotConnected;
state[instance].status = Status::NotConnected;
}
}
@ -383,87 +459,6 @@ bool AP_Proximity::valid_instance(uint8_t i) const
return (Type)params[i].type.get() != Type::None;
}
// detect if an instance of a proximity sensor is connected.
void AP_Proximity::detect_instance(uint8_t instance)
{
switch (get_type(instance)) {
case Type::None:
return;
case Type::RPLidarA2:
if (AP_Proximity_RPLidarA2::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance]);
return;
}
break;
case Type::MAV:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_MAV(*this, state[instance], params[instance]);
return;
case Type::TRTOWER:
if (AP_Proximity_TeraRangerTower::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance]);
return;
}
break;
case Type::TRTOWEREVO:
if (AP_Proximity_TeraRangerTowerEvo::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance], params[instance]);
return;
}
break;
case Type::RangeFinder:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance], params[instance]);
return;
case Type::SF40C:
if (AP_Proximity_LightWareSF40C::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance]);
return;
}
break;
case Type::SF45B:
if (AP_Proximity_LightWareSF45B::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance], params[instance]);
return;
}
break;
case Type::CYGBOT_D1:
#if AP_PROXIMITY_CYGBOT_ENABLED
if (AP_Proximity_Cygbot_D1::detect(instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance]);
return;
}
# endif
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Type::SITL:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_SITL(*this, state[instance], params[instance]);
return;
case Type::AirSimSITL:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]);
return;
#endif
}
}
AP_Proximity *AP_Proximity::_singleton;
namespace AP {

2
libraries/AP_Proximity/AP_Proximity.h
View File

@ -170,8 +170,6 @@ private:
// return true if the given instance exists
bool valid_instance(uint8_t i) const;
void detect_instance(uint8_t instance);
// parameters for all instances
AP_Int8 _raw_log_enable; // enable logging raw distances
AP_Int8 _ign_gnd_enable; // true if land detection should be enabled

16
libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp
View File

@ -25,22 +25,24 @@
*/
AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state,
AP_Proximity_Params &_params) :
AP_Proximity_Params &_params,
uint8_t serial_instance) :
AP_Proximity_Backend(_frontend, _state, _params)
{
const AP_SerialManager &serial_manager = AP::serialmanager();
_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, serial_instance);
if (_uart != nullptr) {
// start uart with larger receive buffer
_uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0), rxspace(), 0);
_uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, serial_instance), rxspace(), 0);
}
}
// detect if a proximity sensor is connected by looking for a
// configured serial port
bool AP_Proximity_Backend_Serial::detect(uint8_t instance)
// static detection function
// detect if a proximity sensor is connected by looking for a configured serial port
// serial_instance affects which serial port is used. Should be 0 or 1 depending on whether this is the 1st or 2nd proximity sensor with a serial interface
bool AP_Proximity_Backend_Serial::detect(uint8_t serial_instance)
{
return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Lidar360, instance);
return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Lidar360, serial_instance);
}
#endif // HAL_PROXIMITY_ENABLED

7
libraries/AP_Proximity/AP_Proximity_Backend_Serial.h
View File

@ -9,10 +9,13 @@ class AP_Proximity_Backend_Serial : public AP_Proximity_Backend
public:
AP_Proximity_Backend_Serial(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state,
AP_Proximity_Params& _params);
AP_Proximity_Params& _params,
uint8_t serial_instance);
// static detection function
static bool detect(uint8_t instance);
// detect if a proximity sensor is connected by looking for a configured serial port
// serial_instance affects which serial port is used. Should be 0 or 1 depending on whether this is the 1st or 2nd proximity sensor with a serial interface
static bool detect(uint8_t serial_instance);
protected:
virtual uint16_t rxspace() const { return 0; };

5
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h
View File

@ -12,8 +12,9 @@ public:
// constructor
AP_Proximity_LightWareSF45B(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state,
AP_Proximity_Params& _params) :
AP_Proximity_LightWareSerial(_frontend, _state, _params) {}
AP_Proximity_Params& _params,
uint8_t serial_instance) :
AP_Proximity_LightWareSerial(_frontend, _state, _params, serial_instance) {}
uint16_t rxspace() const override {
return 1280;