orb: add optional queuing of messages

This adds two uORB API calls:
- orb_advertise_queue
- orb_advertise_multi_queue

Both add a queue_size parameter to define a maximum number of buffered
item. The existing orb calls use all a queue size of one and thus their
behavior is unchanged. If a writer publishes too fast, the oldest elements
from the queue are silently dropped.
The returned timestamp is always the one from the latest message in the
queue.

Queue size can be set via ioctl during advertisement phase. After that it
cannot be changed anymore.

src/modules/uORB/uORBDevices_nuttx.cpp

@@ -50,7 +50,8 @@ uORB::DeviceNode::DeviceNode
 	const struct orb_metadata *meta,
 	const char *name,
 	const char *path,
-	int priority
+	int priority,
+	unsigned int queue_size
 ) :
 	CDev(name, path),
 	_meta(meta),
@@ -60,6 +61,7 @@ uORB::DeviceNode::DeviceNode
 	_publisher(0),
 	_priority(priority),
 	_published(false),
+	_queue_size(queue_size),
 	_IsRemoteSubscriberPresent(false),
 	_subscriber_count(0)
 {
@@ -185,13 +187,26 @@ uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen)
 	 */
 	irqstate_t flags = px4_enter_critical_section();
 
-	/* if the caller doesn't want the data, don't give it to them */
-	if (nullptr != buffer) {
-		memcpy(buffer, _data, _meta->o_size);
+	if (_generation > sd->generation + _queue_size) {
+		/* Reader is too far behind: some messages are lost */
+		sd->generation = _generation - _queue_size;
 	}
 
-	/* track the last generation that the file has seen */
-	sd->generation = _generation;
+	if (_generation == sd->generation && sd->generation > 0) {
+		/* The subscriber already read the latest message, but nothing new was published yet.
+		 * Return the previous message
+		 */
+		--sd->generation;
+	}
+
+	/* if the caller doesn't want the data, don't give it to them */
+	if (nullptr != buffer) {
+		memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size);
+	}
+
+	if (sd->generation < _generation) {
+		++sd->generation;
+	}
 
 	/* set priority */
 	sd->priority = _priority;
@@ -226,7 +241,7 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen)
 
 			/* re-check size */
 			if (nullptr == _data) {
-				_data = new uint8_t[_meta->o_size];
+				_data = new uint8_t[_meta->o_size * _queue_size];
 			}
 
 			unlock();
@@ -245,10 +260,11 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen)
 
 	/* Perform an atomic copy. */
 	irqstate_t flags = px4_enter_critical_section();
-	memcpy(_data, buffer, _meta->o_size);
+	memcpy(_data + (_meta->o_size * (_generation % _queue_size)), buffer, _meta->o_size);
 
 	/* update the timestamp and generation count */
 	_last_update = hrt_absolute_time();
+	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
 	_generation++;
 
 	_published = true;
@@ -290,6 +306,11 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		*(int *)arg = sd->priority;
 		return OK;
 
+	case ORBIOCSETQUEUESIZE:
+		//no need for locking here, since this is used only during the advertisement call,
+		//and only one advertiser is allowed to open the DeviceNode at the same time.
+		return update_queue_size(arg);
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
@@ -522,6 +543,20 @@ bool uORB::DeviceNode::is_published()
 	return _published;
 }
 
+int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
+{
+	if (_queue_size == queue_size) {
+		return PX4_OK;
+	}
+
+	if (_data || _queue_size > queue_size) {
+		return ERROR;
+	}
+
+	_queue_size = queue_size;
+	return PX4_OK;
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)