fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 02:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCAN Sensors: Improve error handling

Browse Source 
Also add new 'generic' device types for UAVCAN sensors

Other misc. cleanup and style changes.
This commit is contained in:
JacobCrabill
2020-03-21 12:39:29 -04:00
committed by Daniel Agar
parent 693a47fca3
commit 1e5784609f
7 changed files with 67 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/sensors/sensor_bridge.cpp
+6 -14
View File
@@ -88,7 +88,7 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
// No such channel - try to create one
if (channel == nullptr) {
if (_out_of_channels) {
return; // Give up immediately - saves some CPU time
return; // Give up immediately - saves some CPU time
}
DEVICE_LOG("adding channel %d...", node_id);
@@ -111,9 +111,6 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
// update device id as we now know our device node_id
_device_id.devid_s.address = static_cast<uint8_t>(node_id);
// initialize the driver, which registers the class device name and uORB publisher
// int ret = init_driver(channel);
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
@@ -124,11 +121,6 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
return;
}
// if (ret != PX4_OK) {
// DEVICE_LOG("INIT ERROR node %d errno %d", channel->node_id, ret);
// return;
// }
// Publish to the appropriate topic, abort on failure
channel->node_id = node_id;
channel->class_instance = class_instance;
@@ -137,17 +129,16 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_VERY_HIGH);
if (channel->orb_advert == nullptr) {
DEVICE_LOG("ADVERTISE FAILED");
DEVICE_LOG("uORB advertise failed. Out of instances?");
(void)unregister_class_devname(_class_devname, class_instance);
*channel = uavcan_bridge::Channel();
_out_of_channels = true;
return;
}
DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance);
DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->orb_instance);
}
//publish_sensor()
assert(channel != nullptr);
(void)orb_publish(_orb_topic, channel->orb_advert, report);
@@ -168,7 +159,8 @@ uavcan_bridge::Channel *UavcanCDevSensorBridgeBase::get_channel_for_node(int nod
// No such channel - try to create one
if (channel == nullptr) {
if (_out_of_channels) {
return channel; // Give up immediately - saves some CPU time
// We already determined we're out of class or uORB instances
return channel;
}
DEVICE_LOG("adding channel %d...", node_id);