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: