Merged beta into master

src/modules/uavcan/sensors/baro.cpp

@@ -45,9 +45,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
 	_sub_air_pressure_data(node),
 	_sub_air_temperature_data(node),
 	_reports(nullptr)
-{
-	last_temperature = 0.0f;
-}
+{ }
 
 int UavcanBarometerBridge::init()
 {
@@ -153,7 +151,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 void UavcanBarometerBridge::air_temperature_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
 {
-	last_temperature = msg.static_temperature;
+	last_temperature_kelvin = msg.static_temperature;
 }
 
 void UavcanBarometerBridge::air_pressure_sub_cb(const
@@ -162,7 +160,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 	baro_report report;
 
 	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
-	report.temperature = last_temperature;
+	report.temperature = last_temperature_kelvin - 273.15F;
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 	report.error_count = 0;
 

src/modules/uavcan/sensors/baro.hpp

@@ -78,6 +78,6 @@ private:
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
 	unsigned _msl_pressure = 101325;
 	ringbuffer::RingBuffer	*_reports;
-	float last_temperature;
+	float last_temperature_kelvin = 0.0f;
 
 };

src/modules/uavcan/sensors/gnss.cpp

@@ -46,20 +46,21 @@
 const char *const UavcanGnssBridge::NAME = "gnss";
 
 UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
-_node(node),
-_sub_fix(node),
-_report_pub(nullptr)
+	_node(node),
+	_sub_fix(node),
+	_report_pub(nullptr)
 {
 }
 
 int UavcanGnssBridge::init()
 {
 	int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
-	if (res < 0)
-	{
+
+	if (res < 0) {
 		warnx("GNSS fix sub failed %i", res);
 		return res;
 	}
+
 	return res;
 }
 
@@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const
 void UavcanGnssBridge::print_status() const
 {
 	printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
+
 	if (_receiver_node_id < 0) {
 		printf("N/A\n");
+
 	} else {
 		printf("%d\n", _receiver_node_id);
 	}
@@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	if (_receiver_node_id < 0) {
 		_receiver_node_id = msg.getSrcNodeID().get();
 		warnx("GNSS receiver node ID: %d", _receiver_node_id);
+
 	} else {
 		if (_receiver_node_id != msg.getSrcNodeID().get()) {
 			return;  // This GNSS receiver is the redundant one, ignore it.
@@ -114,14 +118,15 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 		// Vertical position uncertainty
 		report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
+
 	} else {
 		report.eph = -1.0F;
 		report.epv = -1.0F;
 	}
 
 	if (valid_velocity_covariance) {
-	    float vel_cov[9];
-	    msg.velocity_covariance.unpackSquareMatrix(vel_cov);
+		float vel_cov[9];
+		msg.velocity_covariance.unpackSquareMatrix(vel_cov);
 		report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
 
 		/* There is a nonlinear relationship between the velocity vector and the heading.
@@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 		float vel_n_sq = vel_n * vel_n;
 		float vel_e_sq = vel_e * vel_e;
 		report.c_variance_rad =
-				(vel_e_sq * vel_cov[0] +
-						-2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
-						vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
+			(vel_e_sq * vel_cov[0] +
+			 -2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
+			 vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
 
 	} else {
 		report.s_variance_m_s = -1.0F;
@@ -154,7 +159,8 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	report.vel_n_m_s = msg.ned_velocity[0];
 	report.vel_e_m_s = msg.ned_velocity[1];
 	report.vel_d_m_s = msg.ned_velocity[2];
-	report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s);
+	report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s *
+			       report.vel_d_m_s);
 	report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
 	report.vel_ned_valid = true;
 

src/modules/uavcan/sensors/mag.cpp

@@ -41,11 +41,11 @@
 
 const char *const UavcanMagnetometerBridge::NAME = "mag";
 
-UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
-UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
-_sub_mag(node)
+UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
+	UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
+	_sub_mag(node)
 {
-	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;     // <-- Why?
 
 	_scale.x_scale = 1.0F;
 	_scale.y_scale = 1.0F;
@@ -55,15 +55,18 @@ _sub_mag(node)
 int UavcanMagnetometerBridge::init()
 {
 	int res = device::CDev::init();
+
 	if (res < 0) {
 		return res;
 	}
 
 	res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
+
 	if (res < 0) {
 		DEVICE_LOG("failed to start uavcan sub: %d", res);
 		return res;
 	}
+
 	return 0;
 }
 
@@ -74,6 +77,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
 
 	/* buffer must be large enough */
 	unsigned count = buflen / sizeof(struct mag_report);
+
 	if (count < 1) {
 		return -ENOSPC;
 	}
@@ -85,6 +89,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
 		last_read = _report.timestamp;
 		unlock();
 		return sizeof(struct mag_report);
+
 	} else {
 		/* no new data available, warn caller */
 		return -EAGAIN;
@@ -95,25 +100,31 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 {
 	switch (cmd) {
 	case SENSORIOCSQUEUEDEPTH: {
-		return OK;			// Pretend that this stuff is supported to keep APM happy
-	}
+			return OK;			// Pretend that this stuff is supported to keep APM happy
+		}
+
 	case MAGIOCSSCALE: {
-		std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
-		return 0;
-	}
+			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;
-	}
+			std::memcpy(reinterpret_cast<void *>(arg), &_scale, sizeof(_scale));
+			return 0;
+		}
+
 	case MAGIOCSELFTEST: {
-		return 0;           // Nothing to do
-	}
+			return 0;           // Nothing to do
+		}
+
 	case MAGIOCGEXTERNAL: {
-		return 1;           // declare it external rise it's priority and to allow for correct orientation compensation
-	}
+			return 1;           // declare it external rise it's priority and to allow for correct orientation compensation
+		}
+
 	case MAGIOCSSAMPLERATE: {
-		return 0;           // Pretend that this stuff is supported to keep the sensor app happy
-	}
+			return 0;           // Pretend that this stuff is supported to keep the sensor app happy
+		}
+
 	case MAGIOCCALIBRATE:
 	case MAGIOCGSAMPLERATE:
 	case MAGIOCSRANGE:
@@ -121,23 +132,25 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 	case MAGIOCSLOWPASS:
 	case MAGIOCEXSTRAP:
 	case MAGIOCGLOWPASS: {
-		return -EINVAL;
-	}
+			return -EINVAL;
+		}
+
 	default: {
-		return CDev::ioctl(filp, cmd, arg);
-	}
+			return CDev::ioctl(filp, cmd, arg);
+		}
 	}
 }
 
-void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
+void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
+		&msg)
 {
 	lock();
 	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
 	_report.timestamp = msg.getMonotonicTimestamp().toUSec();
 
-	_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
-	_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
-	_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	_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();
 
 	publish(msg.getSrcNodeID().get(), &_report);

src/modules/uavcan/sensors/mag.hpp

@@ -40,7 +40,7 @@
 #include "sensor_bridge.hpp"
 #include <drivers/drv_mag.h>
 
-#include <uavcan/equipment/ahrs/Magnetometer.hpp>
+#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
 
 class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
 {
@@ -57,14 +57,14 @@ private:
 	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
 	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
-	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
+	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
 
 	typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
 		void (UavcanMagnetometerBridge::*)
-		(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &) >
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
 		MagCbBinder;
 
-	uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
+	uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
 	mag_scale _scale = {};
 	mag_report _report =  {};
 };

src/modules/uavcan/sensors/sensor_bridge.cpp

@@ -45,7 +45,7 @@
 /*
  * IUavcanSensorBridge
  */
-void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
+void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
 {
 	list.add(new UavcanBarometerBridge(node));
 	list.add(new UavcanMagnetometerBridge(node));
@@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
 			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
 		}
 	}
+
 	delete [] _channels;
 }
 
@@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 
 		// Ask the CDev helper which class instance we can take
 		const int class_instance = register_class_devname(_class_devname);
+
 		if (class_instance < 0 || class_instance >= int(_max_channels)) {
 			_out_of_channels = true;
 			DEVICE_LOG("out of class instances");
@@ -128,6 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 
 		DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance);
 	}
+
 	assert(channel != nullptr);
 
 	(void)orb_publish(_orb_topic, channel->orb_advert, report);
@@ -136,11 +139,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
 {
 	unsigned out = 0;
+
 	for (unsigned i = 0; i < _max_channels; i++) {
 		if (_channels[i].node_id >= 0) {
 			out += 1;
 		}
 	}
+
 	return out;
 }
 
@@ -152,6 +157,7 @@ void UavcanCDevSensorBridgeBase::print_status() const
 		if (_channels[i].node_id >= 0) {
 			printf("channel %d: node id %d --> class instance %d\n",
 			       i, _channels[i].node_id, _channels[i].class_instance);
+
 		} else {
 			printf("channel %d: empty\n", i);
 		}

src/modules/uavcan/sensors/sensor_bridge.hpp

@@ -45,7 +45,7 @@
 /**
  * A sensor bridge class must implement this interface.
  */
-class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
+class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge *>
 {
 public:
 	static constexpr unsigned MAX_NAME_LEN = 20;
@@ -77,7 +77,7 @@ public:
 	 * Sensor bridge factory.
 	 * Creates all known sensor bridges and puts them in the linked list.
 	 */
-	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
+	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list);
 };
 
 /**
@@ -86,8 +86,7 @@ public:
  */
 class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
 {
-	struct Channel
-	{
+	struct Channel {
 		int node_id              = -1;
 		orb_advert_t orb_advert  = nullptr;
 		int class_instance       = -1;
@@ -104,13 +103,13 @@ protected:
 	static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
 
 	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
-	                           const orb_id_t orb_topic_sensor,
-	                           const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
-	device::CDev(name, devname),
-	_max_channels(max_channels),
-	_class_devname(class_devname),
-	_orb_topic(orb_topic_sensor),
-	_channels(new Channel[max_channels])
+				   const orb_id_t orb_topic_sensor,
+				   const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
+		device::CDev(name, devname),
+		_max_channels(max_channels),
+		_class_devname(class_devname),
+		_orb_topic(orb_topic_sensor),
+		_channels(new Channel[max_channels])
 	{
 		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
 		_device_id.devid_s.bus = 0;