diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 54e1b46387..388873bbcf 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -464,7 +464,7 @@ void AP_Airspeed::init() break; case TYPE_UAVCAN: #if AP_AIRSPEED_UAVCAN_ENABLED - sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i); + sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i, uint32_t(param[i].bus_id.get())); #endif break; case TYPE_NMEA_WATER: @@ -489,7 +489,30 @@ void AP_Airspeed::init() num_sensors = i+1; } } + +#if AP_AIRSPEED_UAVCAN_ENABLED + // we need a 2nd pass for DroneCAN sensors so we can match order by DEVID + // the 2nd pass accepts any devid + for (uint8_t i=0; iget_driver_index(), + _detected_modules[i].node_id, 0); + if (previous_devid != 0 && previous_devid != bus_id) { + // match with previous ID only + continue; + } backend = new AP_Airspeed_UAVCAN(_frontend, _instance); if (backend == nullptr) { - AP::can().log_text(AP_CANManager::LOG_INFO, - LOG_TAG, - "Failed register UAVCAN Airspeed Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); + AP::can().log_text(AP_CANManager::LOG_INFO, + LOG_TAG, + "Failed register UAVCAN Airspeed Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); } else { _detected_modules[i].driver = backend; - AP::can().log_text(AP_CANManager::LOG_INFO, - LOG_TAG, - "Registered UAVCAN Airspeed Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); - backend->set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN, - _detected_modules[i].ap_uavcan->get_driver_index(), - _detected_modules[i].node_id, 0)); + AP::can().log_text(AP_CANManager::LOG_INFO, + LOG_TAG, + "Registered UAVCAN Airspeed Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); + backend->set_bus_id(bus_id); } break; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h index ce8b861e33..e4115c32ef 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h +++ b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h @@ -28,7 +28,7 @@ public: static void subscribe_msgs(AP_UAVCAN* ap_uavcan); - static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance); + static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance, uint32_t previous_devid); private: