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 "cli_monitor.h"
+#include "cli_setpid.h"
+#include "cli_getpid.h"
+// #include "cli_getimu.h"
+enum CommandNameIds{
+	CMD_MONITOR,
+	CMD_GETPID,
+	CMD_SETPID,
+	CMD_GETIMU,
+	MAX_COMMANDS
+};
+
+typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
+static cli_function_ptr cli_functions[] = {
+	&cli_monitor,
+	&cli_getpid,
+	&cli_setpid
+	//&cli_getimu
+};
+
+static char* commandNames[MAX_COMMANDS] = {
+	"monitor",
+	"getpid",
+	"setpid",
+	"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;
+	static int getRollV = 0, getPitchV = 0, getYawV = 0;
+	static int getHeight = 0, getLat = 0, getLong = 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},
+ 		{"rollv",   no_argument,   	&getYawV, 	1},
+ 		{"pitchv",   no_argument,   	&getYawV, 	1},
+ 		{"yawv",   no_argument,   	&getYawV, 	1},
+ 		{"height",   no_argument,   	&getHeight,	1},
+ 		{"lat",   no_argument,   	&getLat, 	1},
+ 		{"long",   no_argument,   	&getLong, 	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.controller = PID_ROLL;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+		pid_data.controller = PID_PITCH;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+		pid_data.controller = PID_YAW;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+	} else {
+		if(getPitch) {
+			pid_data.controller = PID_PITCH;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getRoll) {
+			pid_data.controller = PID_ROLL;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getYaw) {
+			pid_data.controller = 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->controller) {
+		case PID_PITCH :
+			printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_ROLL :
+			printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_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;
+}

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

+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+#include <time.h>
+#include <unistd.h>
+#include <err.h>
+
+#include "cli_monitor.h"
+#include "frontend_tracker.h"
+
+int cli_monitor(struct backend_conn * conn,	int argc, char **argv) {
+	int c, result;
+	int countFlag = 0;
+
+	int count, rate = 10;
+	int forever = 0;
+
+
+ 	while ((c = getopt(argc, argv, "fc:r:")) != -1) {
+		switch(c) {
+			case 'c' :
+				count = atoi(optarg);
+				countFlag = 1;
+				break;
+			case 'r' :
+				rate = atoi(optarg) + 1;
+				break;
+			case 'f' :
+				forever = 1;
+				break;
+			default :
+				break;
+		}
+	}
+
+	if (forever) {
+		for (;;) {
+			struct timespec req;
+			if (rate == 1) {
+				req.tv_sec = 1;
+				req.tv_nsec = 0;
+			} else { 
+				req.tv_sec = 0;
+				req.tv_nsec = 1000000000 / rate;
+			}
+			nanosleep(&req, NULL);
+			monitor(conn);
+		}
+	} else if (countFlag) {
+		for (int i = 0; i < count; i++) {
+			result = monitor(conn);
+
+			struct timespec req;
+			if (rate == 1) {
+				req.tv_sec = 1;
+				req.tv_nsec = 0;
+			} else { 
+				req.tv_sec = 0;
+				req.tv_nsec = 1000000000 / rate;
+			}
+			nanosleep(&req, NULL);
+		}
+	} else {
+		return monitor(conn);
+	}
+
+	return result;
+}
+
+int monitor(struct backend_conn * conn) {
+	/* Get tracker data */
+	struct frontend_tracker_data td;
+	if (frontend_track(conn, &td)) {
+		errx(1, "Error reading tracker data");
+	}
+
+	/* TODO: Get PID constants and status */
+	/* It might be a good idea to only read the pid constants
+	 * every few seconds, so count iterations and only do it if 
+	 * this is every (rate * 2 or 3) pass through the loop
+	 */
+
+	/* Print stuff on screen */
+	/* Assuming a tab width of 8 columns */
+	printf("\033[2J");
+	printf("STATUS: NA\n");
+	printf("CTRLR :\tP\tR\tY\tP_V\tR_V\tY_V\tH\tLAT\tLON\n");
+	printf("  P   :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+	printf("  I   :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+	printf("  D   :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+	printf("PosAtt:\tH\tLAT\tLON\tP\tR\tY\n");
+	printf("      :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+			td.height, td.lateral, td.longitudinal,
+			td.pitch, td.roll, td.yaw);
+
+	return 0;
+}

groundStation/src/cli/cli_monitor.h

+#ifndef CLI_MONITOR_H
+#define CLI_MONITOR_H
+
+#include <time.h>
+
+#include "frontend_getpid.h"
+
+#define SECOND_IN_NANO 1000000000
+
+int cli_monitor(struct backend_conn * conn,	int argc, char **argv);
+
+// Return 0 on success and 1 otherwise
+int monitor(struct backend_conn * conn);
+#endif
\ No newline at end of file

groundStation/src/cli/cli_setpid.h

+#ifndef _CLI_SETPID_H
+#define _CLI_SETPID_H
+
+#include "frontend_setpid.h"
+
+int cli_setpid(struct backend_conn * conn, int argc, char ** argv);
+
+#endif

groundStation/src/frontend/frontend_common.c

+#define _GNU_SOURCE
+
+#include "frontend_common.h"
+#include "config.h"
+
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <unistd.h>
+#include <err.h>
+
+struct backend_conn {
+	FILE * socket;
+
+	size_t len;
+	char * buf;
+};
+
+struct backend_conn * ucart_backendConnect()
+{
+	int s;
+	struct sockaddr_un remote;
+	char str[100];
+
+	if ((s = socket(AF_UNIX, SOCK_STREAM, 0)) == -1) {
+		perror("socket");
+		exit(1);
+	}
+
+	struct backend_conn * conn = NULL;
+
+	printf("Trying to connect...\n");
+
+	remote.sun_family = AF_UNIX;
+	char * sock_env = getenv(SOCKET_ENV);
+	strcpy(remote.sun_path, sock_env ? sock_env : DEFAULT_SOCKET);
+	if (connect(s, (struct sockaddr *)&remote, sizeof(remote))  == -1) {
+		perror("connect");
+		goto fail_final;
+	}
+
+	conn = malloc(sizeof(struct backend_conn));
+	if (conn == NULL) {
+		perror("malloc");
+		goto fail_sock;
+	}
+
+	conn->len = 0;
+	conn->buf = NULL;
+	conn->socket = fdopen(s, "r+");
+	if (conn->socket == NULL) {
+		perror("fdopen");
+		goto fail_malloc_conn;
+	}
+	if (setvbuf(conn->socket, NULL, _IONBF, 0)) {
+		warn("setvbuf");
+	}
+
+	/* success */
+	goto fail_final;
+
+fail_malloc_conn:
+	free(conn);
+	conn = NULL;
+fail_sock:
+	close(s);
+fail_final:
+	return conn;
+}
+
+void ucart_backendDisconnect(struct backend_conn * conn)
+{
+	fclose(conn->socket);
+	if (conn->buf) {
+		free(conn->buf);
+	}
+	free(conn);
+}
+
+char * ucart_backendGetline(struct backend_conn *conn)
+{
+	getline(&conn->buf, &conn->len, conn->socket);
+	return conn->buf;
+}
+
+int ucart_backendWrite(struct backend_conn *conn, const char * line)
+{
+	return fputs(line, conn->socket);
+}

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

+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "frontend_getpid.h"
+#include "pid_common.h"
+
+/* Get a specified PID.
+ *
+ * Example:
+ *
+ * struct frontend_pid_data pid_data;
+ * pid_data.pid = 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) {
+	
+	char line[100] = "";
+	switch (pid_data->controller) {
+		case PID_PITCH :
+			strncpy(line, "getpitchp\ngetpitchd\n", 20);
+			break;
+		case PID_ROLL :
+			strncpy(line, "getrollp\ngetrolld\n", 18);
+			break;
+		case PID_YAW :
+			strncpy(line, "getyawp\ngetyawd\n", 17);
+			break;
+		default :
+			return 1;
+	}
+
+	int size;
+	if((size = ucart_backendWrite(conn, line)) < 0 ) {
+		return 1;
+	}
+
+	return 0;
+}	

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

groundStation/src/frontend/frontend_setpid.c

+#include <err.h>
+#include <stdio.h>
+
+#include "frontend_setpid.h"
+#include "pid_common.h"
+#include "frontend_common.h"
+
+int frontend_setpid(
+		struct backend_conn * conn,
+		struct frontend_pid_data * pid_data,
+		int mask)
+{
+	if (conn == NULL) {
+		return -1;
+	}
+
+	char * controller;
+	switch (pid_data->controller) {
+		case PID_PITCH:
+			controller = "pitch";
+			break;
+		case PID_ROLL:
+			controller = "roll";
+			break;
+		case PID_YAW:
+			controller = "yaw";
+			break;
+			/* What is the "throttle" pid constant? */
+		default:
+			warnx("Unsupported PID constant");
+			return -1;
+	}
+
+	char buffer[2048];
+	/* Set the P, I, and D */
+	if (mask & SET_P) {
+		if (snprintf(buffer, 2048,
+					"set%sp %f\n", 
+					controller, 
+					pid_data->p) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+	if (mask & SET_I) {
+		if (snprintf(buffer, 2048,
+					"set%si %f\n", 
+					controller, 
+					pid_data->i) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+	if (mask & SET_D) {
+		if (snprintf(buffer, 2048,
+					"set%sd %f\n", 
+					controller, 
+					pid_data->d) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+
+	return 0;
+}

groundStation/src/frontend/frontend_setpid.h

+#ifndef _FRONTEND_SETPID_H
+#define _FRONTEND_SETPID_H
+
+#include "pid_common.h"
+#include "frontend_common.h"
+
+int frontend_setpid(
+		struct backend_conn * conn,
+		struct frontend_pid_data * pid_data,
+		int mask);
+
+#define SET_P 0x01
+#define SET_I 0x02
+#define SET_D 0x04
+#define SET_ALL (SET_P | SET_I | SET_D)
+
+#endif

groundStation/src/frontend/frontend_tracker.c

+#include <err.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "frontend_tracker.h"
+
+#define MAGIC "TRACKERDATA"
+
+int frontend_track(
+		struct backend_conn * conn,
+		struct frontend_tracker_data * data)
+{
+	ucart_backendWrite(conn, MAGIC "\n");
+
+	char * line;
+	for (;;) {
+		line = ucart_backendGetline(conn);
+		if (line == NULL) {
+			warnx("Line not returned from backend");
+			return 1;
+		}
+
+		if (strncmp(line, MAGIC, strlen(MAGIC)) == 0) {
+			break;
+		}
+	}
+
+	if (strncmp(line, MAGIC " ERROR", strlen(MAGIC " ERROR")) == 0) {
+		warnx("Backend returned an error: %s", strstr(line, "ERROR"));
+		return 1;
+	}
+
+	/* Line format: Height Lat Long Pitch Roll Yaw */
+	sscanf(line, MAGIC " %lf %lf %lf %lf %lf %lf ",
+			&data->height,
+			&data->lateral,
+			&data->longitudinal,
+			&data->pitch,
+			&data->roll,
+			&data->yaw);
+	return 0;
+}

groundStation/src/frontend/frontend_tracker.h

+#ifndef _FRONTEND_TRACKER_H
+#define _FRONTEND_TRACKER_H
+
+#include "frontend_common.h"
+
+/* Struct containing pos/att data */
+struct frontend_tracker_data {
+		double height;
+		double lateral;
+		double longitudinal;
+		double pitch;
+		double roll;
+		double yaw;
+};
+
+/* Get pos/att data from the tracking system
+ *
+ * conn: IN Connection to quad
+ * data: OUT Data is written to this struct
+ *
+ * Returns 0 on success, nonzero on error
+ *
+ */
+int frontend_track(struct backend_conn * conn, 
+		struct frontend_tracker_data *data);
+
+#endif

groundStation/src/frontend/pid_common.h

+#ifndef PID_COMMON_H
+#define PID_COMMON_H
+
+enum pid_controller {
+	PID_PITCH,
+	PID_ROLL,
+	PID_YAW,
+	PID_PITCH_VELOCITY,
+	PID_ROLL_VELOCITY,
+	PID_YAW_VELOCITY,
+	PID_HEIGHT, /* Z */
+	PID_LAT, /* X */
+	PID_LONG, /* Y */
+	PID_NUM_PIDS
+};
+
+struct frontend_pid_data {
+	enum pid_controller controller;
+
+	float p;
+	float i;
+	float d;
+};
+
+
+#endif