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.c

+#include <stdio.h>
+#include <string.h>
+#include <err.h>
+#include <libgen.h>
+
+#include "cli.h"
+
+int main(int argc, char **argv)
+{
+	int cmdID = -1;
+	char * command;
+	int i , useSymlink = 0;
+	struct backend_conn  *conn;
+
+	command = basename(argv[0]);
+	for(i = 0; i < MAX_COMMANDS; ++i) {
+		if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
+		{
+			cmdID = i;
+			useSymlink = 1;
+		}
+	}
+
+	if(cmdID == -1) {
+		command = argv[1];
+		for(i = 0; i < MAX_COMMANDS; ++i) {
+			if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
+			{
+				cmdID = i;
+			}	
+		}
+	}
+
+	if(cmdID == -1){
+		printf("Could not match input with a command. Please try again...\n");
+		return -1;
+	}
+
+	printf("Parsed Command : %s\n", commandNames[cmdID]);
+	conn = ucart_backendConnect();
+	if(conn == NULL) {
+		return -1;
+	}
+
+	if(useSymlink) {
+		(*cli_functions[cmdID]) (conn, argc, &argv[0]);
+	}else {
+		(*cli_functions[cmdID]) (conn, argc-1, &argv[1]);
+	}
+
+	ucart_backendDisconnect(conn);
+    return 0;
+}

groundStation/src/cli/cli.h

+#ifndef __CLI_H
+#define __CLI_H
+
+#include "frontend_getpid.h"
+// #include "cli_monitor.h"
+// #include "cli_setpid.h"
+#include "cli_getpid.h"
+// #include "cli_getimu.h"
+enum CommandNameIds{
+	CMD_MONITOR,
+	CMD_SETPID,
+	CMD_GETPID,
+	CMD_GETIMU,
+	MAX_COMMANDS
+};
+
+typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
+static cli_function_ptr cli_functions[1] = {
+	// &cli_monitor,
+	// &cli_setpid,
+	&cli_getpid
+	// &cli_getimu
+};
+
+static char* commandNames[MAX_COMMANDS] = {
+	"monitor",
+	"setPid",
+	"getPid",
+	"getImu"
+};
+
+
+#endif

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

groundStation/src/cli/cli_getpid.c

+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include "cli_getpid.h"
+
+int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
+	int c;
+	static int getRoll = 0, getPitch = 0, getYaw = 0, getAll = 0;
+	struct frontend_pid_data pid_data;
+	static struct option long_options[] = {
+ 		/* These options don’t set a flag. We distinguish them by their indices. */
+ 		{"roll",	no_argument,	&getRoll,	1},
+ 		{"pitch",   no_argument,   	&getPitch, 	1},
+ 		{"yaw",   no_argument,   	&getYaw, 	1},
+ 		{0, 0, 0, 0}
+ 	};
+
+ 	while (1)
+	{
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "a", long_options, &option_index);
+
+		/* Detect the end of the options. */
+		if (c == -1)
+			break;
+
+		if (c == 'a') {
+			getAll = 1;
+		}
+	}
+
+	int result;
+	if(getAll) {
+		pid_data.pid = ROLL;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+		pid_data.pid = PITCH;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+		pid_data.pid = YAW;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+	} else {
+		if(getPitch) {
+			pid_data.pid = PITCH;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getRoll) {
+			pid_data.pid = ROLL;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getYaw) {
+			pid_data.pid = YAW;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
+	if(frontend_getpid(conn, pid_data)) {
+		return 1;
+	}
+
+	return 0;
+
+	switch(pid_data->pid) {
+		case PITCH :
+			printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case ROLL :
+			printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case YAW :
+			printf("Yaw Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		default :
+			break;
+	}
+	return 0;
+}
\ No newline at end of file

groundStation/src/cli/cli_getpid.h

+#ifndef CLI_GETPID_H
+#define CLI_GETPID_H
+
+#include "frontend_getpid.h"
+
+int getValues(struct backend_conn *, struct frontend_pid_data *);
+int cli_getpid(struct backend_conn * conn, int argc, char ** argv);
+
+#endif
\ No newline at end of file

groundStation/src/cli/cli_monitor.h

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

groundStation/src/frontend/frontend_common.h

+#ifndef FRONTEND_COMMON_H
+#define FRONTEND_COMMON_H
+#include <stdlib.h>
+
+struct backend_conn;
+
+/* Start connection to quad */
+struct backend_conn * ucart_backendConnect(void);
+
+/* Stop connection to quad */
+void ucart_backendDisconnect(struct backend_conn * conn);
+
+/* Get a line from the backend.
+ *
+ * The line will remain valid until the next call to ucart_backendGetline.
+ */
+char * ucart_backendGetline(struct backend_conn * conn);
+
+/* Write a line to the backend */
+int ucart_backendWrite(struct backend_conn * backend, const char * line);
+
+#endif

groundStation/src/frontend/frontend_getpid.h

+#ifndef FRONTEND_GETPID_H
+#define FRONTEND_GETPID_H
+
+#include "frontend_common.h"
+#include "pid_common.h"
+
+/* Get a specified PID.
+ *
+ * Example:
+ *
+ * struct frontend_pid_data pid_data;
+ * pid_data.pid = PITCH;
+ * if (frontend_getpid(conn, &pid_data)) {
+ * 		error
+ * } else {
+ * 		pid_data.p, pid_data.i, and pid_data.d are filled
+ * }
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getpid(
+		struct backend_conn * conn,
+		struct frontend_pid_data * pid_data);
+
+
+#endif

vrpn @ 99c54dbe

+Subproject commit 99c54dbefe04897cc7c146101a126f62c0e65f97