From e709048fb85278c449938a7c87af3b4c6cb7281e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 15:27:29 +0200
Subject: [PATCH] orb: add orb_get_interval to API

---
 src/drivers/drv_orb_dev.h              |  3 +++
 src/modules/uORB/uORB.cpp              |  6 +++++-
 src/modules/uORB/uORB.h                |  5 +++++
 src/modules/uORB/uORBDevices_nuttx.cpp |  4 ++++
 src/modules/uORB/uORBDevices_posix.cpp |  4 ++++
 src/modules/uORB/uORBManager.cpp       |  8 ++++++++
 src/modules/uORB/uORBManager.hpp       | 12 ++++++++++++
 7 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h
index 710eb3d175..2a0d5ca7d9 100644
--- a/src/drivers/drv_orb_dev.h
+++ b/src/drivers/drv_orb_dev.h
@@ -90,4 +90,7 @@
 /** Set the queue size of the topic */
 #define ORBIOCSETQUEUESIZE	_ORBIOC(15)
 
+/** Get the minimum interval at which the topic can be seen to be updated for this subscription */
+#define ORBIOCGETINTERVAL	_ORBIOC(16)
+
 #endif /* _DRV_UORB_H */
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 4724502ebc..8a0bf0bdfc 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -140,8 +140,12 @@ int  orb_priority(int handle, int32_t *priority)
 	return uORB::Manager::get_instance()->orb_priority(handle, priority);
 }
 
-int  orb_set_interval(int handle, unsigned interval)
+int orb_set_interval(int handle, unsigned interval)
 {
 	return uORB::Manager::get_instance()->orb_set_interval(handle, interval);
 }
 
+int orb_get_interval(int handle, unsigned *interval)
+{
+	return uORB::Manager::get_instance()->orb_get_interval(handle, interval);
+}
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index c1b03c0def..f7f1a9dff4 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -232,6 +232,11 @@ extern int	orb_priority(int handle, int32_t *priority) __EXPORT;
  */
 extern int	orb_set_interval(int handle, unsigned interval) __EXPORT;
 
+/**
+ * @see uORB::Manager::orb_get_interval()
+ */
+extern int	orb_get_interval(int handle, unsigned *interval) __EXPORT;
+
 __END_DECLS
 
 /* Diverse uORB header defines */ //XXX: move to better location
diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp
index e27f55d297..e0f51469a5 100644
--- a/src/modules/uORB/uORBDevices_nuttx.cpp
+++ b/src/modules/uORB/uORBDevices_nuttx.cpp
@@ -311,6 +311,10 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		//and only one advertiser is allowed to open the DeviceNode at the same time.
 		return update_queue_size(arg);
 
+	case ORBIOCGETINTERVAL:
+		*(unsigned *)arg = sd->update_interval;
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp
index c5cd4b8fbd..d828431c56 100644
--- a/src/modules/uORB/uORBDevices_posix.cpp
+++ b/src/modules/uORB/uORBDevices_posix.cpp
@@ -326,6 +326,10 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		//and only one advertiser is allowed to open the DeviceNode at the same time.
 		return update_queue_size(arg);
 
+	case ORBIOCGETINTERVAL:
+		*(unsigned *)arg = sd->update_interval;
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return VDev::ioctl(filp, cmd, arg);
diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp
index e5e669527a..d4d958f8e3 100644
--- a/src/modules/uORB/uORBManager.cpp
+++ b/src/modules/uORB/uORBManager.cpp
@@ -204,6 +204,14 @@ int uORB::Manager::orb_set_interval(int handle, unsigned interval)
 	return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
 }
 
+int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
+{
+	ASSERT(interval);
+	int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval);
+	*interval /= 1000;
+	return ret;
+}
+
 
 int uORB::Manager::node_advertise
 (
diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp
index 9a6a56c48a..e43e57e887 100644
--- a/src/modules/uORB/uORBManager.hpp
+++ b/src/modules/uORB/uORBManager.hpp
@@ -327,6 +327,18 @@ public:
 	 */
 	int  orb_set_interval(int handle, unsigned interval) ;
 
+
+	/**
+	 * Get the minimum interval between which updates are seen for a subscription.
+	 *
+	 * @see orb_set_interval()
+	 *
+	 * @param handle  A handle returned from orb_subscribe.
+	 * @param interval  The returned interval period in milliseconds.
+	 * @return    OK on success, ERROR otherwise with ERRNO set accordingly.
+	 */
+	int	orb_get_interval(int handle, unsigned *interval);
+
 	/**
 	 * Method to set the uORBCommunicator::IChannel instance.
 	 * @param comm_channel