From 4c8cfa140a95ce045c778b84873570f864f38b86 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Fri, 28 Feb 2020 18:06:32 -0800 Subject: [PATCH] UAVCAN: Overhaul SensorBridge drivers Supposedly multiple sensor callbacks were supported; in reality, this was not the case, as the mag SensorBridge in particular can only calibrate one compass, leading to a race condition on which compass appears first on the bus to get published and calibrated (with no warning to the user that the 'wrong' compass is being used). For sensors with existing generic driver classes (baro and mag) the sensor bridges use these classes for the driver registration, and uORB publication, and calibration interface (ioctl) handling. --- src/drivers/uavcan/sensors/baro.cpp | 56 ++++++--- src/drivers/uavcan/sensors/baro.hpp | 2 + src/drivers/uavcan/sensors/mag.cpp | 111 +++++++++--------- src/drivers/uavcan/sensors/mag.hpp | 5 +- src/drivers/uavcan/sensors/sensor_bridge.cpp | 70 ++++++++++- src/drivers/uavcan/sensors/sensor_bridge.hpp | 34 ++++-- src/lib/drivers/barometer/PX4Barometer.hpp | 2 + .../drivers/magnetometer/PX4Magnetometer.hpp | 2 + 8 files changed, 189 insertions(+), 93 deletions(-) diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index 7e6f6b0a90..279314d060 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/src/drivers/uavcan/sensors/baro.cpp @@ -39,10 +39,14 @@ #include "baro.hpp" #include +#include + 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", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), + UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", UAVCAN_BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_pressure_data(node), _sub_air_temperature_data(node) { } @@ -83,24 +87,40 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg) { - sensor_baro_s report{}; + lock(); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + unlock(); - // Set the devid address to the UAVCAN node ID (so we get a unique address) - _device_id.devid_s.address = (uint8_t)(msg.getSrcNodeID().get() & 0xFF); + if (channel == nullptr) { + // Something went wrong - no channel to publish on; return + return; + } - /* - * FIXME HACK - * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT. - * It stopped working when the time sync feature has been introduced, because it caused libuavcan - * to use an independent time source (based on hardware TIM5) instead of HRT. - * The proper solution is to be developed. - */ - report.timestamp = hrt_absolute_time(); - report.temperature = last_temperature_kelvin - 273.15F; - report.pressure = msg.static_pressure / 100.0F; // Convert to millibar - report.error_count = 0; + // Cast our generic CDev pointer to the sensor-specific driver class + PX4Barometer *_baro = (PX4Barometer *)channel->h_driver; - report.device_id = _device_id.devid; - - publish(msg.getSrcNodeID().get(), &report); + _baro->set_temperature(last_temperature_kelvin - 273.15f); + _baro->update(hrt_absolute_time(), msg.static_pressure / 100.0f); // Convert pressure to millibar +} + +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.address = static_cast(channel->node_id); + + channel->h_driver = new PX4Barometer(device_id.devid, ORB_PRIO_HIGH); + + if (channel->h_driver == nullptr) { + return PX4_ERROR; + } + + PX4Barometer *_baro = (PX4Barometer *)channel->h_driver; + + channel->class_instance = _baro->get_class_instance(); + + return PX4_OK; } diff --git a/src/drivers/uavcan/sensors/baro.hpp b/src/drivers/uavcan/sensors/baro.hpp index 53661c0b27..ec2ffe933e 100644 --- a/src/drivers/uavcan/sensors/baro.hpp +++ b/src/drivers/uavcan/sensors/baro.hpp @@ -59,6 +59,8 @@ private: void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg); void air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); + int init_driver(uavcan_bridge::Channel *channel) override; + typedef uavcan::MethodBinder < UavcanBarometerBridge *, void (UavcanBarometerBridge::*) (const uavcan::ReceivedDataStructure &) > diff --git a/src/drivers/uavcan/sensors/mag.cpp b/src/drivers/uavcan/sensors/mag.cpp index 2756be960d..f68973b501 100644 --- a/src/drivers/uavcan/sensors/mag.cpp +++ b/src/drivers/uavcan/sensors/mag.cpp @@ -40,18 +40,17 @@ #include #include +#include + 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", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), + UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", UAVCAN_MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node), _sub_mag2(node) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why? - - _scale.x_scale = 1.0F; - _scale.y_scale = 1.0F; - _scale.z_scale = 1.0F; } int @@ -85,26 +84,6 @@ UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case MAGIOCSSCALE: { - std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); - return 0; - } - - case MAGIOCGSCALE: { - std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); - return 0; - } - - case MAGIOCGEXTERNAL: { - return 1; // declare it external rise it's priority and to allow for correct orientation compensation - } - - case MAGIOCCALIBRATE: - case MAGIOCSRANGE: - case MAGIOCEXSTRAP: { - return -EINVAL; - } - default: { return CDev::ioctl(filp, cmd, arg); } @@ -115,46 +94,62 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) { - lock(); - /* - * FIXME HACK - * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT. - * It stopped working when the time sync feature has been introduced, because it caused libuavcan - * to use an independent time source (based on hardware TIM5) instead of HRT. - * The proper solution is to be developed. - */ - _report.timestamp = hrt_absolute_time(); - _report.device_id = _device_id.devid; - _report.is_external = true; + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); - _report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale; - _report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale; - _report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale; - unlock(); + if (channel == nullptr) { + // Something went wrong - no channel to publish on; return + return; + } - publish(msg.getSrcNodeID().get(), &_report); + // Cast our generic CDev pointer to the sensor-specific driver class + PX4Magnetometer *_mag = (PX4Magnetometer *)channel->h_driver; + + 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); } void UavcanMagnetometerBridge::mag2_sub_cb(const uavcan::ReceivedDataStructure &msg) { - lock(); - /* - * FIXME HACK - * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT. - * It stopped working when the time sync feature has been introduced, because it caused libuavcan - * to use an independent time source (based on hardware TIM5) instead of HRT. - * The proper solution is to be developed. - */ - _report.timestamp = hrt_absolute_time(); - _report.device_id = _device_id.devid; - _report.is_external = true; + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); - _report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale; - _report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale; - _report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale; - unlock(); + if (channel == nullptr) { + // Something went wrong - no channel to publish on; return + return; + } - publish(msg.getSrcNodeID().get(), &_report); + // Cast our generic CDev pointer to the sensor-specific driver class + PX4Magnetometer *_mag = (PX4Magnetometer *)channel->h_driver; + + 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); +} + +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.address = static_cast(channel->node_id); + + channel->h_driver = new PX4Magnetometer(device_id.devid, ORB_PRIO_HIGH, ROTATION_NONE); + + if (channel->h_driver == nullptr) { + return PX4_ERROR; + } + + PX4Magnetometer *_mag = (PX4Magnetometer *)channel->h_driver; + _mag->set_external(true); + channel->class_instance = _mag->get_class_instance(); + + return PX4_OK; } diff --git a/src/drivers/uavcan/sensors/mag.hpp b/src/drivers/uavcan/sensors/mag.hpp index 04e1208270..51425b9f9b 100644 --- a/src/drivers/uavcan/sensors/mag.hpp +++ b/src/drivers/uavcan/sensors/mag.hpp @@ -59,6 +59,8 @@ private: int ioctl(struct file *filp, int cmd, unsigned long arg) override; + int init_driver(uavcan_bridge::Channel *channel) override; + void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); void mag2_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -74,7 +76,4 @@ private: uavcan::Subscriber _sub_mag; uavcan::Subscriber _sub_mag2; - - mag_calibration_s _scale{}; - sensor_mag_s _report{}; }; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index e9ea0ad643..a258434135 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -73,7 +73,9 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) { - Channel *channel = nullptr; + assert(report != nullptr); + + uavcan_bridge::Channel *channel = nullptr; // Checking if such channel already exists for (unsigned i = 0; i < _max_channels; i++) { @@ -109,6 +111,9 @@ 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); @@ -119,29 +124,86 @@ 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; + DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance); 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"); (void)unregister_class_devname(_class_devname, class_instance); - *channel = Channel(); + *channel = uavcan_bridge::Channel(); return; } DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance); } + //publish_sensor() + assert(channel != nullptr); (void)orb_publish(_orb_topic, channel->orb_advert, report); } -unsigned -UavcanCDevSensorBridgeBase::get_num_redundant_channels() const +uavcan_bridge::Channel *UavcanCDevSensorBridgeBase::get_channel_for_node(int node_id) +{ + uavcan_bridge::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 channel; // Give up immediately - saves some CPU time + } + + DEVICE_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; + DEVICE_LOG("out of channels"); + return channel; + } + + // initialize the driver, which registers the class device name and uORB publisher + channel->node_id = node_id; + int ret = init_driver(channel); + + if (ret != PX4_OK) { + DEVICE_LOG("INIT ERROR node %d errno %d", channel->node_id, ret); + return channel; + } + + DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance); + } + + return channel; +} + +unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.hpp b/src/drivers/uavcan/sensors/sensor_bridge.hpp index 8ec20f6797..04384a3980 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.hpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include /** @@ -81,26 +82,30 @@ public: static void make_all(uavcan::INode &node, List &list); }; +namespace uavcan_bridge +{ +struct Channel { + int node_id = -1; + orb_advert_t orb_advert = nullptr; + int class_instance = -1; + int orb_instance = -1; + cdev::CDev *h_driver = nullptr; +}; +} + /** * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel. * For example, sensor_mag0, sensor_mag1, etc. */ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev { - struct Channel { - int node_id = -1; - orb_advert_t orb_advert = nullptr; - int class_instance = -1; - int orb_instance = -1; - }; - const char *const _class_devname; const orb_id_t _orb_topic; - Channel *const _channels; + uavcan_bridge::Channel *const _channels; bool _out_of_channels = false; protected: - static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; + static constexpr unsigned DEFAULT_MAX_CHANNELS = ORB_MULTI_MAX_INSTANCES; const unsigned _max_channels; UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, @@ -109,7 +114,7 @@ protected: device::CDev(name, devname), _class_devname(class_devname), _orb_topic(orb_topic_sensor), - _channels(new Channel[max_channels]), + _channels(new uavcan_bridge::Channel[max_channels]), _max_channels(max_channels) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; @@ -124,6 +129,15 @@ protected: */ void publish(const int node_id, const void *report); + /** + * Init the sensor driver for this channel. + * Implementation depends on sensor type being constructed. + * @param channel Channel pointer for which h_driver should be initialized. + */ + virtual int init_driver(uavcan_bridge::Channel *channel) { return PX4_OK; }; + + uavcan_bridge::Channel *get_channel_for_node(int node_id); + public: virtual ~UavcanCDevSensorBridgeBase(); diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index 11179a19a2..6103e055be 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -57,6 +57,8 @@ public: void update(hrt_abstime timestamp, float pressure); + int get_class_instance(void) { return _class_device_instance; }; + void print_status(); private: diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 2bfa23d6ee..6ec6ae4bd5 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -60,6 +60,8 @@ public: void update(hrt_abstime timestamp_sample, float x, float y, float z); + int get_class_instance(void) { return _class_device_instance; }; + void print_status(); private: