diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 31b433cd1f..f40b6381a8 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -151,6 +151,18 @@ #define DRV_SENS_DEVTYPE_PCF8583 0x7e #define DRV_TEL_DEVTYPE_BST 0x7f +// Generic types for unknown CAN sensors +#define DRV_ACC_DEVTYPE_UAVCAN 0x80 +#define DRV_BARO_DEVTYPE_UAVCAN 0x81 +#define DRV_BAT_DEVTYPE_UAVCAN 0x82 +#define DRV_DIFF_PRESS_DEVTYPE_UAVCAN 0x83 +#define DRV_FLOW_DEVTYPE_UAVCAN 0x84 +#define DRV_GPS_DEVTYPE_UAVCAN 0x85 +#define DRV_GYR_DEVTYPE_UAVCAN 0x86 +#define DRV_IMU_DEVTYPE_UAVCAN 0x87 +#define DRV_MAG_DEVTYPE_UAVCAN 0x88 +#define DRV_DIST_DEVTYPE_UAVCAN 0x89 + #define DRV_DEVTYPE_UNUSED 0xff /* diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index c4e668dc47..d070d7b7d4 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/src/drivers/uavcan/sensors/baro.cpp @@ -47,7 +47,8 @@ const char *const UavcanBarometerBridge::NAME = "baro"; #define UAVCAN_BARO_BASE_DEVICE_PATH "/dev/uavcan/baro" UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", UAVCAN_BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), + UavcanCDevSensorBridgeBase("uavcan_baro", UAVCAN_BARO_BASE_DEVICE_PATH, UAVCAN_BARO_BASE_DEVICE_PATH, + ORB_ID(sensor_baro)), _sub_air_pressure_data(node), _sub_air_temperature_data(node) { } @@ -98,10 +99,14 @@ UavcanBarometerBridge::air_pressure_sub_cb(const } // Cast our generic CDev pointer to the sensor-specific driver class - PX4Barometer *_baro = (PX4Barometer *)channel->h_driver; + PX4Barometer *baro = (PX4Barometer *)channel->h_driver; - _baro->set_temperature(last_temperature_kelvin + CONSTANTS_ABSOLUTE_NULL_CELSIUS); - _baro->update(hrt_absolute_time(), msg.static_pressure / 100.0f); // Convert pressure to millibar + if (baro == nullptr) { + return; + } + + baro->set_temperature(last_temperature_kelvin + CONSTANTS_ABSOLUTE_NULL_CELSIUS); + baro->update(hrt_absolute_time(), msg.static_pressure / 100.0f); // Convert pressure to millibar } int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel) @@ -109,8 +114,7 @@ int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel) // update device id as we now know our device node_id DeviceId device_id{_device_id}; - // No sensor info is included int he StaticPressure msg; use some generic baro type - device_id.devid_s.devtype = DRV_BARO_DEVTYPE_MS5611; + device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN; device_id.devid_s.address = static_cast(channel->node_id); channel->h_driver = new PX4Barometer(device_id.devid, ORB_PRIO_HIGH); @@ -119,9 +123,16 @@ int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel) return PX4_ERROR; } - PX4Barometer *_baro = (PX4Barometer *)channel->h_driver; + PX4Barometer *baro = (PX4Barometer *)channel->h_driver; - channel->class_instance = _baro->get_class_instance(); + channel->class_instance = baro->get_class_instance(); + + if (channel->class_instance < 0) { + PX4_ERR("UavcanBaro: Unable to get a class instance"); + delete baro; + channel->h_driver = nullptr; + return PX4_ERROR; + } return PX4_OK; } diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index 5ec09f779f..166be10a7d 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -68,7 +68,7 @@ UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure micros diff --git a/src/drivers/uavcan/sensors/mag.cpp b/src/drivers/uavcan/sensors/mag.cpp index f68973b501..f973a7a90c 100644 --- a/src/drivers/uavcan/sensors/mag.cpp +++ b/src/drivers/uavcan/sensors/mag.cpp @@ -47,7 +47,7 @@ const char *const UavcanMagnetometerBridge::NAME = "mag"; #define UAVCAN_MAG_BASE_DEVICE_PATH "/dev/uavcan/mag" UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", UAVCAN_MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), + UavcanCDevSensorBridgeBase("uavcan_mag", UAVCAN_MAG_BASE_DEVICE_PATH, UAVCAN_MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node), _sub_mag2(node) { @@ -102,13 +102,17 @@ UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructureh_driver; + PX4Magnetometer *mag = (PX4Magnetometer *)channel->h_driver; + + if (mag == nullptr) { + return; + } const float x = msg.magnetic_field_ga[0]; const float y = msg.magnetic_field_ga[1]; const float z = msg.magnetic_field_ga[2]; - _mag->update(hrt_absolute_time(), x, y, z); + mag->update(hrt_absolute_time(), x, y, z); } void @@ -117,19 +121,23 @@ UavcanMagnetometerBridge::mag2_sub_cb(const { uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); - if (channel == nullptr) { + if (channel == nullptr || channel->class_instance < 0) { // Something went wrong - no channel to publish on; return return; } // Cast our generic CDev pointer to the sensor-specific driver class - PX4Magnetometer *_mag = (PX4Magnetometer *)channel->h_driver; + PX4Magnetometer *mag = (PX4Magnetometer *)channel->h_driver; + + if (mag == nullptr) { + return; + } const float x = msg.magnetic_field_ga[0]; const float y = msg.magnetic_field_ga[1]; const float z = msg.magnetic_field_ga[2]; - _mag->update(hrt_absolute_time(), x, y, z); + mag->update(hrt_absolute_time(), x, y, z); } int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel) @@ -137,8 +145,7 @@ int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel) // update device id as we now know our device node_id DeviceId device_id{_device_id}; - // No sensor info is included in the MagneticFieldStrength msg; use some generic mag type - device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; + device_id.devid_s.devtype = DRV_MAG_DEVTYPE_UAVCAN; device_id.devid_s.address = static_cast(channel->node_id); channel->h_driver = new PX4Magnetometer(device_id.devid, ORB_PRIO_HIGH, ROTATION_NONE); @@ -147,9 +154,18 @@ int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel) return PX4_ERROR; } - PX4Magnetometer *_mag = (PX4Magnetometer *)channel->h_driver; - _mag->set_external(true); - channel->class_instance = _mag->get_class_instance(); + PX4Magnetometer *mag = (PX4Magnetometer *)channel->h_driver; + + channel->class_instance = mag->get_class_instance(); + + if (channel->class_instance < 0) { + PX4_ERR("UavcanMag: Unable to get a class instance"); + delete mag; + channel->h_driver = nullptr; + return PX4_ERROR; + } + + mag->set_external(true); return PX4_OK; } diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index a258434135..7a0e9842a2 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -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(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); diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index 6103e055be..a2de6b5108 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -57,7 +57,7 @@ public: void update(hrt_abstime timestamp, float pressure); - int get_class_instance(void) { return _class_device_instance; }; + int get_class_instance() { return _class_device_instance; }; void print_status(); diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 6ec6ae4bd5..213774f641 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -60,7 +60,7 @@ public: void update(hrt_abstime timestamp_sample, float x, float y, float z); - int get_class_instance(void) { return _class_device_instance; }; + int get_class_instance() { return _class_device_instance; }; void print_status();