AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass)
{
AP_Compass_UAVCAN *sensor = nullptr;
if (AP_BoardConfig_CAN::get_can_num_ifaces() != 0) {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
if (uavcan != nullptr) {
uint8_t freemag = uavcan->find_smallest_free_mag_node();
if (freemag != UINT8_MAX) {
sensor = new AP_Compass_UAVCAN(compass);
if (sensor->register_uavcan_compass(i, freemag)) {
debug_mag_uavcan(2, "AP_Compass_UAVCAN probed, drv: %d, node: %d\n\r", i, freemag);
return sensor;
} else {
delete sensor;
sensor = nullptr;
}
}
}
}
}
}
return sensor;
}
AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass)
{
if (AP_BoardConfig_CAN::get_can_num_ifaces() == 0) {
return nullptr;
}
AP_Compass_UAVCAN *sensor;
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
if (ap_uavcan == nullptr) {
continue;
}
uint8_t freemag = ap_uavcan->find_smallest_free_mag_node();
if (freemag == UINT8_MAX) {
continue;
}
sensor = new AP_Compass_UAVCAN(compass);
if (sensor->register_uavcan_compass(i, freemag)) {
debug_mag_uavcan(2, "AP_Compass_UAVCAN probed, drv: %d, node: %d\n\r", i, freemag);
return sensor;
} else {
delete sensor;
}
}
return nullptr;
}