ADIS16477_gyro::~ADIS16477_gyro() { if (_gyro_class_instance != -1) { unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); } }
LSM303D_mag::~LSM303D_mag() { if (_mag_class_instance != -1) unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance); }
void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) { assert(report != nullptr); Channel *channel = nullptr; // Checking if such channel already exists for (unsigned i = 0; i < _max_channels; i++) { if (_channels[i].node_id == node_id) { channel = _channels + i; break; } } // No such channel - try to create one if (channel == nullptr) { if (_out_of_channels) { return; // Give up immediately - saves some CPU time } log("adding channel %d...", node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) { if (_channels[i].node_id < 0) { channel = _channels + i; break; } } // No free channels left if (channel == nullptr) { _out_of_channels = true; log("out of channels"); return; } // update device id as we now know our device node_id _device_id.devid_s.address = static_cast<uint8_t>(node_id); // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { _out_of_channels = true; log("out of class instances"); (void)unregister_class_devname(_class_devname, class_instance); return; } // Publish to the appropriate topic, abort on failure channel->node_id = node_id; channel->class_instance = class_instance; channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); *channel = Channel(); return; } log("channel %d class instance %d ok", channel->node_id, channel->class_instance); } assert(channel != nullptr); (void)orb_publish(_orb_topic, channel->orb_advert, report); }