src/vrpn_tracker.cpp → groundStation/src/backend/vrpn_tracker.cpp

 #include <iostream>
 #include <algorithm>
+#include <functional>
 
 #include "vrpn_Tracker.h"
 #include "quat.h"
@@ -9,28 +10,25 @@
 namespace microcart
 {
 	static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);
+	
+	TrackerData::TrackerData() :
+		x(0.0), y(0.0), z(0.0),
+		pitch(0.0), roll(0.0), yaw(0.0),
+		fps(0.0), timestamp()
+	{
+	}
 
-	Tracker::Tracker(std::string server) : Tracker(server.c_str())
+	Tracker::Tracker(std::string server) : 
+		remote(server.c_str()),
+		stop_flag(0),
+		trackerData()
 	{
 	}
 
 	Tracker::Tracker(const char * server) :
 		remote(server),
 		stop_flag(0),
-		trackerData({
-			.x = 0.0,
-			.y = 0.0,
-			.z = 0.0,
-			.pitch = 0.0,
-			.roll = 0.0,
-			.yaw = 0.0,
-			.fps = 0.0,
-			.timestamp = {
-				.tv_sec = 0,
-				.tv_usec = 0
-			}
-		})
-
+		trackerData()
 	{
 		remote.register_change_handler(this, vrpn_cb);
 
@@ -92,10 +90,9 @@ namespace microcart
 		trackerData.fps = 1.0 / elapsed_time_usec;
 
 		auto td = trackerData;
-		std::for_each(cb_vector.begin(), cb_vector.end(),
-			[td](std::function<void(const TrackerData &)> &fn){
-				fn(td);
-		});
+		for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
+			(*i)(td);
+		}
 	}
 
 	void Tracker::addCallback(std::function<void(const TrackerData&)> cb) 
@@ -115,6 +112,20 @@ struct ucart_vrpn_tracker {
 	microcart::Tracker * t;
 };
 
+void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
+		const microcart::TrackerData &td)
+{
+						struct ucart_vrpn_TrackerData data;
+						data.x = td.x;
+						data.y = td.y;
+						data.z = td.z;
+						data.pitch = td.pitch;
+						data.roll = td.roll;
+						data.yaw = td.yaw;
+						data.fps = td.fps;
+						(*cb)(&data);
+}
+
 extern "C"
 {
 	struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
@@ -123,6 +134,7 @@ extern "C"
 		try {
 			auto inst = new struct ucart_vrpn_tracker;
 			inst->t = new microcart::Tracker(server);
+			return inst;
 		} catch(...) {
 			return NULL;
 		}
@@ -138,16 +150,8 @@ extern "C"
 			void (*cb)(struct ucart_vrpn_TrackerData *))
 	{
 		try {
-			inst->t->addCallback([cb](const microcart::TrackerData & td) {
-						struct ucart_vrpn_TrackerData data;
-						data.x = td.x;
-						data.y = td.y;
-						data.z = td.z;
-						data.pitch = td.pitch;
-						data.roll = td.roll;
-						data.yaw = td.yaw;
-						(*cb)(&data);
-					});
+			auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
+			inst->t->addCallback(new_cb);
 		} catch(...) {
 			return -1;
 		}

src/vrpn_tracker.hpp → groundStation/src/backend/vrpn_tracker.hpp

@@ -54,6 +54,7 @@ extern "C"
 namespace microcart
 {
 	struct TrackerData {
+	public:
 		double x;
 		double y;
 		double z;
@@ -64,6 +65,8 @@ namespace microcart
 
 		double fps;
 		timeval timestamp;
+
+		TrackerData();
 	};
 
 	class Tracker {

groundStation/src/cli/cli_getimu.h

+#ifndef CLI_GETIMU_H
+#define CLI_GETIMU_H
+#include "frontend_getimu.h"
+
+int cli_getimu(
+		struct backend_conn * conn,
+		struct frontend_pid_data * pid_data);
+
+#endif
\ No newline at end of file