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;
}