Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-11 10:23:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Rangefinder: support multiple serial rangefinders

Browse Source
This commit is contained in:
Francisco Ferreira 2018-02-23 12:11:47 +00:00 committed by Randy Mackay
parent 5999421c72
commit 45531775cd
10 changed files with 49 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -25,12 +25,13 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder already know that we should setup the rangefinder
*/ */
AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) : AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state) AP_RangeFinder_Backend(_state)
{ {
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance);
if (uart != nullptr) { if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0)); uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, serial_instance));
} }
} }
@ -39,9 +40,9 @@ AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_Stat
trying to take a reading on Serial. If we get a result the sensor is trying to take a reading on Serial. If we get a result the sensor is
there. there.
*/ */
bool AP_RangeFinder_LeddarOne::detect(AP_SerialManager &serial_manager) bool AP_RangeFinder_LeddarOne::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
{ {
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0) != nullptr; return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance) != nullptr;
} }
// read - return last value measured by sensor // read - return last value measured by sensor

5
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -43,10 +43,11 @@ class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend
public: public:
// constructor // constructor
AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager); AP_SerialManager &serial_manager,
uint8_t serial_instance);
// static detection function // static detection function
static bool detect(AP_SerialManager &serial_manager); static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
// update state // update state
void update(void); void update(void);

11
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -26,12 +26,13 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder already know that we should setup the rangefinder
*/ */
AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) : AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state) AP_RangeFinder_Backend(_state)
{ {
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance);
if (uart != nullptr) { if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0)); uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, serial_instance));
} }
} }
@ -40,9 +41,9 @@ AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::Rang
trying to take a reading on Serial. If we get a result the sensor is trying to take a reading on Serial. If we get a result the sensor is
there. there.
*/ */
bool AP_RangeFinder_LightWareSerial::detect(AP_SerialManager &serial_manager) bool AP_RangeFinder_LightWareSerial::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
{ {
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0) != nullptr; return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance) != nullptr;
} }
// read - return last value measured by sensor // read - return last value measured by sensor

5
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
View File

@ -9,10 +9,11 @@ class AP_RangeFinder_LightWareSerial : public AP_RangeFinder_Backend
public: public:
// constructor // constructor
AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager); AP_SerialManager &serial_manager,
uint8_t serial_instance);
// static detection function // static detection function
static bool detect(AP_SerialManager &serial_manager); static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
// update state // update state
void update(void); void update(void);

11
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp
View File

@ -30,12 +30,13 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder already know that we should setup the rangefinder
*/ */
AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) : AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state) AP_RangeFinder_Backend(_state)
{ {
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance);
if (uart != nullptr) { if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0)); uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, serial_instance));
} }
} }
@ -44,9 +45,9 @@ AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder::Ra
trying to take a reading on Serial. If we get a result the sensor is trying to take a reading on Serial. If we get a result the sensor is
there. there.
*/ */
bool AP_RangeFinder_MaxsonarSerialLV::detect(AP_SerialManager &serial_manager) bool AP_RangeFinder_MaxsonarSerialLV::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
{ {
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0) != nullptr; return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance) != nullptr;
} }
// read - return last value measured by sensor // read - return last value measured by sensor

5
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
View File

@ -9,10 +9,11 @@ class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend
public: public:
// constructor // constructor
AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager); AP_SerialManager &serial_manager,
uint8_t serial_instance);
// static detection function // static detection function
static bool detect(AP_SerialManager &serial_manager); static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
// update state // update state
void update(void); void update(void);

9
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp
View File

@ -33,10 +33,11 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder already know that we should setup the rangefinder
*/ */
AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) : AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state) AP_RangeFinder_Backend(_state)
{ {
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance);
if (uart != nullptr) { if (uart != nullptr) {
uart->begin(ULANDING_BAUD, ULANDING_BUFSIZE_RX, ULANDING_BUFSIZE_TX); uart->begin(ULANDING_BAUD, ULANDING_BUFSIZE_RX, ULANDING_BUFSIZE_TX);
} }
@ -47,9 +48,9 @@ AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State
trying to take a reading on Serial. If we get a result the sensor is trying to take a reading on Serial. If we get a result the sensor is
there. there.
*/ */
bool AP_RangeFinder_uLanding::detect(AP_SerialManager &serial_manager) bool AP_RangeFinder_uLanding::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
{ {
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0) != nullptr; return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, serial_instance) != nullptr;
} }
/* /*

5
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h
View File

@ -9,10 +9,11 @@ class AP_RangeFinder_uLanding : public AP_RangeFinder_Backend
public: public:
// constructor // constructor
AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager); AP_SerialManager &serial_manager,
uint8_t serial_instance);
// static detection function // static detection function
static bool detect(AP_SerialManager &serial_manager); static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
// update state // update state
void update(void); void update(void);

24
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -553,8 +553,10 @@ void RangeFinder::init(void)
// init called a 2nd time? // init called a 2nd time?
return; return;
} }
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) { for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
detect_instance(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) { if (drivers[i] != nullptr) {
// we loaded a driver for this instance, so it must be // we loaded a driver for this instance, so it must be
// present (although it may not be healthy) // present (although it may not be healthy)
@ -607,7 +609,7 @@ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
/* /*
detect if an instance of a rangefinder is connected. detect if an instance of a rangefinder is connected.
*/ */
void RangeFinder::detect_instance(uint8_t instance) void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
{ {
enum RangeFinder_Type _type = (enum RangeFinder_Type)state[instance].type.get(); enum RangeFinder_Type _type = (enum RangeFinder_Type)state[instance].type.get();
switch (_type) { switch (_type) {
@ -671,21 +673,21 @@ void RangeFinder::detect_instance(uint8_t instance)
break; break;
#endif #endif
case RangeFinder_TYPE_LWSER: case RangeFinder_TYPE_LWSER:
if (AP_RangeFinder_LightWareSerial::detect(serial_manager)) { if (AP_RangeFinder_LightWareSerial::detect(serial_manager, serial_instance)) {
state[instance].instance = instance; state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], serial_manager); drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], serial_manager, serial_instance++);
} }
break; break;
case RangeFinder_TYPE_LEDDARONE: case RangeFinder_TYPE_LEDDARONE:
if (AP_RangeFinder_LeddarOne::detect(serial_manager)) { if (AP_RangeFinder_LeddarOne::detect(serial_manager, serial_instance)) {
state[instance].instance = instance; state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], serial_manager); drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], serial_manager, serial_instance++);
} }
break; break;
case RangeFinder_TYPE_ULANDING: case RangeFinder_TYPE_ULANDING:
if (AP_RangeFinder_uLanding::detect(serial_manager)) { if (AP_RangeFinder_uLanding::detect(serial_manager, serial_instance)) {
state[instance].instance = instance; state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_uLanding(state[instance], serial_manager); drivers[instance] = new AP_RangeFinder_uLanding(state[instance], serial_manager, serial_instance++);
} }
break; break;
#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \ #if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
@ -704,9 +706,9 @@ void RangeFinder::detect_instance(uint8_t instance)
} }
break; break;
case RangeFinder_TYPE_MBSER: case RangeFinder_TYPE_MBSER:
if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_manager)) { if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_manager, serial_instance)) {
state[instance].instance = instance; state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], serial_manager); drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], serial_manager, serial_instance++);
} }
break; break;
case RangeFinder_TYPE_ANALOG: case RangeFinder_TYPE_ANALOG:

2
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -167,7 +167,7 @@ private:
AP_SerialManager &serial_manager; AP_SerialManager &serial_manager;
Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests
void detect_instance(uint8_t instance); void detect_instance(uint8_t instance, uint8_t& serial_instance);
void update_instance(uint8_t instance); void update_instance(uint8_t instance);
bool _add_backend(AP_RangeFinder_Backend *driver); bool _add_backend(AP_RangeFinder_Backend *driver);