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

AP_RangeFinder: fixed mixing UAVCAN and non-UAVCAN rangefinders 

UAVCAN rangefinders add themselves to the frontend drivers as the
devices appear. If they turn up before RangeFinder::init() is run then
this prevented init() from scanning for the other rangefinders as
num_instances is non-zero

This also fixes a race condition in updating num_instances in the
UAVCAN backend
This commit is contained in:
Andrew Tridgell 2020-05-01 08:09:27 +10:00
parent 70f445e7f1
commit d16f31711e
3 changed files with 33 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -255,10 +255,11 @@ void RangeFinder::convert_params(void) {
*/
void RangeFinder::init(enum Rotation orientation_default)
{
if (num_instances != 0) {
if (init_done) {
// init called a 2nd time?
return;
}
init_done = true;
convert_params();
@ -270,11 +271,14 @@ void RangeFinder::init(enum Rotation 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
WITH_SEMAPHORE(detect_sem);
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;
// present (although it may not be healthy). We use MAX()
// here as a UAVCAN rangefinder may already have been
// found
num_instances = MAX(num_instances, i+1);
}
// initialise status
@ -502,6 +506,18 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
drivers[instance] = new AP_RangeFinder_LeddarVu8(state[instance], params[instance], serial_instance++);
}
break;
#if HAL_WITH_UAVCAN
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);
break;
#endif
default:
break;
}

2
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -185,6 +185,8 @@ private:
RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
uint8_t num_instances;
bool init_done;
HAL_Semaphore detect_sem;
float estimated_terrain_height;
Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests

21
libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp
View File

@ -49,11 +49,12 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u
return nullptr;
}
AP_RangeFinder_UAVCAN* driver = nullptr;
RangeFinder &frontend = *AP::rangefinder();
//Scan through the Rangefinder params to find UAVCAN RFND with matching address.
for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
if ((RangeFinder::Type)AP::rangefinder()->params[i].type.get() == RangeFinder::Type::UAVCAN &&
AP::rangefinder()->params[i].address == address) {
driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i];
if ((RangeFinder::Type)frontend.params[i].type.get() == RangeFinder::Type::UAVCAN &&
frontend.params[i].address == address) {
driver = (AP_RangeFinder_UAVCAN*)frontend.drivers[i];
}
//Double check if the driver was initialised as UAVCAN Type
if (driver != nullptr && (driver->_backend_type == RangeFinder::Type::UAVCAN)) {
@ -70,19 +71,21 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u
if (create_new) {
for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
if ((RangeFinder::Type)AP::rangefinder()->params[i].type.get() == RangeFinder::Type::UAVCAN &&
AP::rangefinder()->params[i].address == address) {
if (AP::rangefinder()->drivers[i] != nullptr) {
if ((RangeFinder::Type)frontend.params[i].type.get() == RangeFinder::Type::UAVCAN &&
frontend.params[i].address == address) {
WITH_SEMAPHORE(frontend.detect_sem);
if (frontend.drivers[i] != nullptr) {
//we probably initialised this driver as something else, reboot is required for setting
//it up as UAVCAN type
return nullptr;
}
AP::rangefinder()->drivers[i] = new AP_RangeFinder_UAVCAN(AP::rangefinder()->state[i], AP::rangefinder()->params[i]);
driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i];
frontend.drivers[i] = new AP_RangeFinder_UAVCAN(frontend.state[i], frontend.params[i]);
driver = (AP_RangeFinder_UAVCAN*)frontend.drivers[i];
if (driver == nullptr) {
break;
}
AP::rangefinder()->num_instances = MAX(i+1, AP::rangefinder()->num_instances);
gcs().send_text(MAV_SEVERITY_INFO, "RangeFinder[%u]: added UAVCAN node %u addr %u",
unsigned(i), unsigned(node_id), unsigned(address));
//Assign node id and respective uavcan driver, for identification
if (driver->_ap_uavcan == nullptr) {
driver->_ap_uavcan = ap_uavcan;