mavlink: major rewrite, prepare for dynamic mavlink streams configuration, WIP

1 files changed, 47 insertions, 0 deletions

diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
new file mode 100644
index 000000000..182407a5e
--- /dev/null
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -0,0 +1,47 @@
+/*
+ * mavlink_orb_subscription.cpp
+ *
+ *  Created on: 23.02.2014
+ *      Author: ton
+ */
+
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <uORB/uORB.h>
+
+#include "mavlink_orb_subscription.h"
+
+MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size)
+{
+	this->meta = meta;
+	this->data = malloc(size);
+	memset(this->data, 0, size);
+	this->fd = orb_subscribe(meta);
+	this->last_update = 0;
+	this->interval = 0;
+}
+
+MavlinkOrbSubscription::~MavlinkOrbSubscription()
+{
+	close(fd);
+	free(data);
+}
+
+int MavlinkOrbSubscription::set_interval(const unsigned int interval)
+{
+	this->interval = interval;
+	return orb_set_interval(fd, interval);
+}
+
+int MavlinkOrbSubscription::update(const hrt_abstime t)
+{
+	if (last_update != t) {
+		bool updated;
+		orb_check(fd, &updated);
+		if (updated)
+			return orb_copy(meta, fd, &data);
+	}
+	return OK;
+}