zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Proximity: fixup detection of serial drivers

master
Randy Mackay 3 years ago committed by Andrew Tridgell
parent
826cf558eb
commit
965cdd7e72
5 changed files with 98 additions and 99 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 167
      libraries/AP_Proximity/AP_Proximity.cpp
  2. 2
      libraries/AP_Proximity/AP_Proximity.h
  3. 16
      libraries/AP_Proximity/AP_Proximity_Backend_Serial.cpp
  4. 7
      libraries/AP_Proximity/AP_Proximity_Backend_Serial.h
  5. 5
      libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h

167
libraries/AP_Proximity/AP_Proximity.cpp
Unescape View File

@ -109,16 +109,92 @@ void AP_Proximity::init() @@ -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 @@ -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
Unescape View File

@ -170,8 +170,6 @@ private: @@ -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
Unescape View File

@ -25,22 +25,24 @@ @@ -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
Unescape View File

@ -9,10 +9,13 @@ class AP_Proximity_Backend_Serial : public AP_Proximity_Backend @@ -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
Unescape View File

@ -12,8 +12,9 @@ public: @@ -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;

Write Preview
Loading…
Cancel
Save