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

@@ -39,10 +39,14 @@
 #include "baro.hpp"
 #include <math.h>
 
+#include <lib/drivers/barometer/PX4Barometer.hpp>
+
 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<uavcan::equipment::air_data::StaticPressure> &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<uint8_t>(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;
 }

src/drivers/uavcan/sensors/baro.hpp

@@ -59,6 +59,8 @@ private:
 	void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);
 	void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);
 
+	int init_driver(uavcan_bridge::Channel *channel) override;
+
 	typedef uavcan::MethodBinder < UavcanBarometerBridge *,
 		void (UavcanBarometerBridge::*)
 		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) >

src/drivers/uavcan/sensors/mag.cpp

@@ -40,18 +40,17 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+
 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<const void *>(arg), sizeof(_scale));
-			return 0;
-		}
-
-	case MAGIOCGSCALE: {
-			std::memcpy(reinterpret_cast<void *>(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<uavcan::equipment::ahrs::MagneticFieldStrength>
 				     &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<uavcan::equipment::ahrs::MagneticFieldStrength2> &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<uint8_t>(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;
 }

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<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
 	void mag2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &msg);
 
@@ -74,7 +76,4 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
 	uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength2, Mag2CbBinder> _sub_mag2;
-
-	mag_calibration_s _scale{};
-	sensor_mag_s _report{};
 };

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<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);
 
@@ -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;
 

src/drivers/uavcan/sensors/sensor_bridge.hpp

@@ -41,6 +41,7 @@
 #include <uavcan/uavcan.hpp>
 #include <drivers/device/device.h>
 #include <drivers/drv_orb_dev.h>
+#include <lib/cdev/CDev.hpp>
 #include <uORB/uORB.h>
 
 /**
@@ -81,26 +82,30 @@ public:
 	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &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();
 

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:

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: