Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_RangeFinder: fixed mixing UAVCAN and non-UAVCAN rangefinders #14246

Merged
merged 1 commit into from
May 11, 2020
Merged

AP_RangeFinder: fixed mixing UAVCAN and non-UAVCAN rangefinders #14246

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 19 additions & 3 deletions libraries/AP_RangeFinder/AP_RangeFinder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand All @@ -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
Expand Down Expand Up @@ -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;
}
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_RangeFinder/AP_RangeFinder.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -180,6 +180,8 @@ class RangeFinder
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

Expand Down
21 changes: 12 additions & 9 deletions libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand All @@ -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;
Expand Down