groundStation/src/backend/backend.c

+/* Author: Kris Burney & Jake Drahos
+ *
+ * BlueTooth socket program for passing vrpn data to quad.
+ */
+
+#define _GNU_SOURCE
+#define _BSD_SOURCE
+
+//system includes
+#include <err.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <signal.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <sys/select.h>
+#include <sys/stat.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/rfcomm.h>
+#include <pthread.h>
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <netinet/tcp.h>
+
+//user created includes
+#include "communication.h"
+#include "commands.h"
+#include "vrpn_tracker.hpp"
+#include "type_def.h"
+#include "logger.h"
+#include "config.h"
+
+#define QUAD_BT_ADDR  "00:06:66:64:61:D6"
+#define QUAD_BT_CHANNEL  0x01
+#define CMD_MAX_LENGTH 1024
+#define MAX_HASH_SIZE 50
+
+/* Backend-internal command magics */
+#define TD_MAGIC "TRACKERDATA"
+
+// function prototypes
+void readAndPrint(void);
+void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
+void sendStartPacket(void);
+void getVRPNPacket(struct ucart_vrpn_TrackerData *);
+void printVrpnData(struct ucart_vrpn_TrackerData *);
+int connectToZybo();
+int safe_fd_set(int , fd_set* , int* );
+int safe_fd_clr(int , fd_set* , int* );
+static void safe_close_fd(int fd, pthread_mutex_t *mutexLock);
+
+static void cb(struct ucart_vrpn_TrackerData *);
+static int new_client(int fd);
+/* Return index of client, or -1 */
+static ssize_t get_client_index(int fd);
+/* Returns pointer to client buffer, or -1 */
+static char * get_client_buffer(int fd);
+/* Return pointer to client pending responses, or -1*/ 
+static int * get_client_pend_responses(int fd);
+/* Return positive integer if successful, -1 otherwise */
+static int clientAddPendResponses(int fd, unsigned char *packet);
+/* Returns -1 on error */
+static int remove_client(int fd);
+/* Receive data from client */
+static void client_recv(int fd);
+/* Receive data from quad */
+static void quad_recv();
+/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
+static int wasDisconnected(int fd);
+
+/* Thread-safe wrappers */
+pthread_mutex_t quadSocketMutex;
+static ssize_t writeQuad(const char * buf, size_t count);
+static ssize_t readQuad(char * buf, size_t count);
+
+/* Functions for recording Latencies */
+void findTimeDiff(int respID);
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y);
+//time stamp checking
+static unsigned int currMessageID = 0;
+struct timeval timeArr[MAX_HASH_SIZE];
+
+// global variables
+static volatile int keepRunning = 1;
+const char *TRACKER_IP = "UAV@192.168.0.120:3883";
+static int zyboSocket;
+static int backendSocket;
+struct ucart_vrpn_tracker * tracker = NULL;
+const char *logHeader = "";//"#\n#\tDefault log header\n#\tEverything after '#'`s will be printed as is in the processed logs.\n#\n\0";
+
+#define MAX_CLIENTS 32
+#define CLIENT_BUFFER_SIZE 1024
+#define CLIENT_MAX_PENDING_RESPONSES 5
+static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE];
+static int client_fds[MAX_CLIENTS];
+static int client_pending_responses[MAX_CLIENTS][CLIENT_MAX_PENDING_RESPONSES];
+fd_set rfds_master;
+int max_fd = 0;
+
+pthread_mutex_t quadResponseMutex, cliInputMutex ;
+unsigned char *commandBuf;
+int newQuadResponse = 0, newCliInput = 0;
+
+// Structures to be used throughout
+modular_structs_t structs;
+
+// Callback to be ran whenever the tracker receives data.
+// Currently doing much more than it should. It will be slimmed down 
+// 		in the future.
+static void cb(struct ucart_vrpn_TrackerData * td) {
+	static int count = 0;
+
+	if(!(count % 10)) {
+		sendVrpnPacket(td);
+		//updateLogFile(td);
+	}
+	count++;
+}
+
+int main(int argc, char **argv)
+{
+	int activity;
+	FD_ZERO(&rfds_master);
+
+	/* 
+	 * Create backend listening socket
+	 */
+	/* Determine socket path */
+	char * backend_socket_path = DEFAULT_SOCKET;
+	if (getenv(SOCKET_ENV)) {
+		backend_socket_path = getenv(SOCKET_ENV);
+	}
+	/* Unlink if it exists */
+	unlink(backend_socket_path);
+
+	/* Create socket */
+	mode_t old_umask = umask(0111);
+	backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
+	if (backendSocket < 0) {
+		err(-1, "socket");
+	}
+
+	/* Create sockaddr and bind */
+	struct sockaddr_un sa;
+	sa.sun_family = AF_UNIX;
+	strncpy(sa.sun_path, backend_socket_path, 107);
+	sa.sun_path[107] = '\0';
+	if (bind(backendSocket, (struct sockaddr *) &sa, sizeof(sa))) {
+		err(-1, "bind");
+	}
+	umask(old_umask);
+
+	/* Listen */
+	if (listen(backendSocket, 16)) {
+		err(-1, "listen");
+	}
+
+	/* Add to socket set */
+	safe_fd_set(backendSocket, &rfds_master, &max_fd);
+	
+
+	/* Initialize client buffers */
+	for (int i = 0; i < MAX_CLIENTS; i++) {
+		client_fds[i] = -1;
+		client_buffers[i][0] = '\n';
+		for(int j = 0; j < CLIENT_MAX_PENDING_RESPONSES; j++) {
+			client_pending_responses[i][j] = -1;
+		}
+	}
+
+	if (pthread_mutex_lock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	if ((zyboSocket = connectToZybo()) < 0)
+	{
+		perror("Error connecting to Quad...");
+		free(commandBuf);
+		exit(1);
+	}
+
+	if (pthread_mutex_unlock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+	
+	// open the log file
+	if(createLogFile(argc, argv[1]))
+	{
+		perror("Error creating log file...");
+		exit(1);
+	}
+	// writeStringToLog(logHeader);
+
+	// watch for input from stdin (fd 0) to see when it has input
+	safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
+
+	if (!getenv(NOQUAD_ENV)) {
+		// watch for input from the zybo socket
+		safe_fd_set(zyboSocket, &rfds_master, &max_fd);
+
+	}
+	// Tell the quad we are ready to send it vrpn data
+	sendStartPacket();
+
+	if(!getenv(NOVRPN_ENV)){
+		// create vrpnTracker instance
+		tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
+		// this function will be called whenever tracker receives data
+		ucart_vrpn_tracker_addCallback(tracker, cb);	
+	}
+
+	struct timeval timeout = {
+		.tv_sec = 1,
+		.tv_usec = 0
+	};
+
+	while(keepRunning)
+	{
+		fd_set rfds;
+		rfds = rfds_master;
+		activity = select(max_fd+1, &rfds, NULL, NULL, NULL);
+		if(activity == -1) {
+			perror("select() ");
+		} else if (activity) {
+			for(int fd = 0; fd <= max_fd; ++fd) {
+				if (FD_ISSET(fd, &rfds)) {
+					if(wasDisconnected(fd)){
+						break;
+					}
+					if (fd == fileno(stdin)) {
+						/**
+						 * Ignore stdin from the backend
+						 */
+					} else if (fd == zyboSocket) {
+						printf("recieving from quad\n");
+						quad_recv();
+					} else if (fd == backendSocket) {
+						int new_fd = 0;
+						new_fd = accept(backendSocket, NULL, NULL);
+						if (new_fd < 0) {
+							warn("accept");
+						} else {
+							printf("Connection\n");
+							if (new_client(new_fd)) {
+								printf("Added client\n");
+								safe_fd_set(new_fd, &rfds_master, &max_fd);
+							}
+						}
+					} else if (get_client_index(fd) > -1) {
+						/* It is a socket to a frontend */
+						client_recv(fd);
+					}
+				}
+			}
+		} else {
+			timeout.tv_sec = 1;
+			timeout.tv_usec = 0;
+		}
+	}
+
+	ucart_vrpn_tracker_freeInstance(tracker);
+	
+	safe_close_fd(zyboSocket, &quadSocketMutex);
+
+	closeLogFile();
+	return 0;
+}
+
+void sendStartPacket() {
+	unsigned char packet[8] = {0};
+	currMessageID++;
+	metadata_t metadata = 
+	{
+		(char) BEGIN_CHAR,
+		0x04,
+		0x01,
+		0x01,
+		0
+	};
+
+	packet[0] = metadata.begin_char; // BEGIN					//PACKET_START_BYTE;
+	packet[1] = metadata.msg_type;	// UPDATE				//'U'; // U for vrpn camera update, C for command
+	packet[2] = metadata.msg_subtype; // BEGIN UPDATE
+	packet[3] = (currMessageID & 0x000000ff); 			// MSG ID(1)
+	packet[4] =	((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
+	packet[5] = 0; 			// DATALEN(1)
+	packet[6] = 0; 	// DATALEN(2)
+	
+	char checksum = 0;
+	int i;
+	for(i=0; i < metadata.data_len + 7; i++)
+		checksum ^= packet[i];
+
+	packet[metadata.data_len + 7] = checksum; //PACKET_END_BYTE;
+	int status = writeQuad((char * ) packet, metadata.data_len + 8);
+	if (status != 8)
+	{
+		perror("Error sending start packet...\n");
+		keepRunning = 0;
+	}else
+	{
+		printf("Start packet successfuly sent...\n");
+	}	
+}
+
+void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
+	int pSize = 36;//sizeof(struct ucart_vrpn_TrackerData) + 8;
+	int n;
+	unsigned char packet[pSize];
+	currMessageID++;
+	packet[0] = 0xBE; // BEGIN					//PACKET_START_BYTE;
+	packet[1] = 0x04; // UPDATE				//'U'; // U for vrpn camera update, C for command
+	packet[2] = 0x00; // N/A
+	//TODO Figure out Packet ID with this new ucar_vrpn_TrackerData struct
+	packet[3] = (currMessageID & 0x000000ff); 			// MSG ID(1)
+	packet[4] =	((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
+	packet[5] = (28 & 0x000000ff); 			// DATALEN(1)
+	packet[6] = ((28) >> 8 & 0x00000ff); 	// DATALEN(2)
+	//memcpy(&packet[7], &info, sizeof(struct ucart_vrpn_TrackerData));
+
+	memset(&packet[7], 0, 4);
+	memcpy(&packet[11], &(info->y), 4);
+	memcpy(&packet[15], &(info->x), 4);
+	memcpy(&packet[19], &(info->z), 4);
+	memcpy(&packet[23], &(info->pitch), 4);
+	memcpy(&packet[27], &(info->roll), 4);
+	memcpy(&packet[31], &(info->yaw), 4);
+
+	char checksum = 0;
+	int i;
+	for(i=0; i < pSize - 1; i++) 
+		checksum ^= packet[i];
+
+	packet[pSize - 1] = checksum; //PACKET_END_BYTE;
+
+	n = writeQuad((char *) packet, pSize);
+	if(n < 0) {	
+		perror("vrpnhandler: ERROR writing to socket");
+		keepRunning = 0;
+	}
+
+	struct timeval tstart;
+	gettimeofday(&tstart, NULL);
+	timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
+}
+
+void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
+	int status;
+	if((status = ucart_vrpn_tracker_getData(tracker, td)) < 0)
+	{
+		perror("Error receiving VRPN data from tracker...");
+		keepRunning = 0;
+	}
+}
+
+void printVrpnData(struct ucart_vrpn_TrackerData * td) {
+	printf("FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
+		td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
+}
+
+int connectToZybo() {
+	int sock;
+	int status = 0;
+
+	if (getenv(NOQUAD_ENV)) {
+		return 0;
+	}
+
+	/* Use bluetooth by default */
+	if (!getenv(QUAD_WIFI_ENV)) {
+		printf("Using BT Settings\n");
+		struct sockaddr_rc addr;
+
+		// allocate a socket	
+		sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+
+		//set the connection params ie. who to connect to	
+		addr.rc_family = AF_BLUETOOTH;
+		addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
+		str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
+		
+		printf("Attempting to connect to zybo. Please be patient...\n");
+		// blocking call to connect to socket sock ie. zybo board
+		status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+	} else {
+		printf("Using WIFI settings\n");
+		struct sockaddr_in addr;
+
+		addr.sin_family = AF_INET;
+
+		/* Quick and Dirty */
+		if (getenv(QUAD_IP_ENV)) {
+			 if (!inet_aton(getenv(QUAD_IP_ENV), &addr.sin_addr)) {
+				printf("Env var %s invalid IP %s\n",
+					     	QUAD_IP_ENV, getenv(QUAD_IP_ENV));
+				return -1;
+			 }
+		} else {
+			if (!inet_aton(QUAD_IP_DEFAULT, &addr.sin_addr)) {
+				printf("Default IP %s is invalid\n",
+					     	QUAD_IP_DEFAULT);
+				return -1;
+			}
+		}
+
+		if (getenv(QUAD_PORT_ENV)) {
+			/* Quick 'n dirty, oh yeah! */
+			addr.sin_port = htons(atoi(getenv(QUAD_PORT_ENV)));		
+		} else {
+			addr.sin_port = htons(QUAD_PORT_DEFAULT);
+		}
+
+		sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
+		if (sock < 0) {
+			perror("socket");
+			return -1;
+		}
+		printf("Connecting to Quad @ %s:%u\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port));
+
+		status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+	}
+
+	// connection failed
+	if(status < 0)
+	{
+		close(sock);
+		perror("connect");
+		return -1;
+	}
+	else
+	{
+		// int result = setsockopt(sock, IPPROTO_TCP, TCP_NODELAY, (int[]){1}, sizeof(int));
+		// printf("result = %d\n", result);
+		printf("connection successful!...\n");
+		return sock;
+	}
+}
+
+/* add a fd to fd_set, and update max_fd */
+int safe_fd_set(int fd, fd_set* fds, int* max_fd) {
+    assert(max_fd != NULL);
+
+    FD_SET(fd, fds);
+    if (fd > *max_fd) {
+        *max_fd = fd;
+    }
+    return 0;
+}
+
+/* clear fd from fds, update max fd if needed */
+int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
+    assert(max_fd != NULL);
+
+    FD_CLR(fd, fds);
+    if (fd == *max_fd) {
+        (*max_fd)--;
+    }
+    return 0;
+}
+
+static ssize_t writeQuad(const char * buf, size_t count) {
+	ssize_t retval;
+	if (getenv(NOQUAD_ENV)) {
+		return count;
+	}
+	if (pthread_mutex_lock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+	retval = write(zyboSocket, buf, count);
+	if (pthread_mutex_unlock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	return retval;
+}
+
+static ssize_t readQuad(char * buf, size_t count) {
+	ssize_t retval;
+	if (pthread_mutex_lock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+	retval = read(zyboSocket, buf, count);
+	if (pthread_mutex_unlock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+	return retval;
+}
+
+static int new_client(int fd) {
+	ssize_t new_slot = -1;
+	for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
+		if (client_fds[i] < 0) {
+			new_slot = i;
+			break;
+		}
+	}
+	if (new_slot == -1) {
+		warnx("Ran out of room! Consider increasing MAX_CLIENTS!");
+		return 0;
+	}
+
+	client_fds[new_slot] = fd;
+	client_buffers[new_slot][0] = '\0';
+
+	return 1;
+}
+
+static ssize_t get_client_index(int fd) {
+	for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
+		if (client_fds[i] == fd) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+static char * get_client_buffer(int fd) {
+	ssize_t slot = get_client_index(fd);
+	if (slot == -1) {
+		return NULL;
+	} else {
+		return client_buffers[slot];
+	}
+}
+
+static int * get_client_pend_responses(int fd) {
+	ssize_t slot = get_client_index(fd);
+	if (slot == -1) {
+		return NULL;
+	} else {
+		return client_pending_responses[slot];
+	}
+}
+
+static int clientAddPendResponses(int fd, unsigned char *packet) {
+	int *pendingResponses = get_client_pend_responses(fd);
+	int packetID = (packet[4] << 8) | (packet[3]);
+	for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
+		if(pendingResponses[i] == -1) {
+			pendingResponses[i] = packetID;
+			return i;
+		}
+	}
+	return -1;
+}
+
+static int remove_client(int fd) {
+	ssize_t slot = get_client_index(fd);
+	if(slot == -1)
+		return -1;
+	char *clientBuffer = get_client_buffer(fd);
+	if(clientBuffer == NULL)
+		return -1;
+	clientBuffer[0] = '\0';
+	int *pendingResponses = get_client_pend_responses(fd);
+	if(pendingResponses == NULL)
+		return -1;
+	for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
+		pendingResponses[i] = -1;
+	}
+	client_fds[slot] = -1;
+	return 0;
+}
+
+static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
+	if (pthread_mutex_lock(mutexLock)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+	close(fd);
+	if (pthread_mutex_unlock(mutexLock)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+}
+
+static void client_recv(int fd) {
+	char * buffer;
+	ssize_t len_pre;
+     	buffer = get_client_buffer(fd);
+	len_pre = strlen(buffer);
+
+	char * cursor;
+	cursor = buffer + len_pre;
+
+	ssize_t r;
+	r = read(fd, cursor, CLIENT_BUFFER_SIZE - len_pre - 1);
+	if (r < 0) {
+		warn("read (fd: %d)", fd);
+	}
+	buffer[len_pre + r] = '\0';
+
+	int index = 0;
+	/* Parse buffer and handle commands */
+	while (1) {
+		/* not using strtok because reasons */
+		size_t len = strlen(buffer);
+		ssize_t newline = -1;
+		for (size_t i = 0; i < len; i++) {
+			if (buffer[i] == '\n') {
+				newline = i;
+				break;
+			}
+		}
+
+		/* No newline found. End parsing */
+		if (newline == -1) {
+			break;
+		}
+		buffer[newline] = '\0';
+		
+		printf("Client(%d) : '%s'\n",fd, buffer);
+		unsigned char * packet;
+		if(formatCommand(buffer, &packet) == -1) {
+			/* buffer was not a quad command, handling internally to
+			 * backend instead of forwarding to quad
+			 */
+			if (strncmp(buffer, TD_MAGIC, strlen(TD_MAGIC)) == 0) {
+				/* Request for tracker data */
+				struct ucart_vrpn_TrackerData td;
+				if (tracker == NULL) {
+					char * dummy = TD_MAGIC " 1.0 2.0 3.0 4.0 5.0 6.0\n";
+					write(fd, dummy, strlen(dummy));
+				}else if (ucart_vrpn_tracker_getData(tracker, &td)) {
+					write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
+				} else { 
+					/* more than sufficient buffer */
+					char buffer[2048];
+					/* Map VRPN XYZ to Height Lat Long (right now it's
+					 * a guess). Format is Height Lat Long P R Y */
+					if (snprintf(buffer,
+							2048,
+							TD_MAGIC " %lf %lf %lf %lf %lf %lf\n",
+							td.z,
+							td.y,
+							td.x,
+							td.pitch,
+							td.roll,
+							td.yaw) >= 2048) {
+
+						/* Output longer than buffer */
+						warnx("Increase format buffer size, output was too long!");
+						write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
+					}
+					write(fd, buffer, strlen(buffer));
+				}
+			}
+		} else {
+			if(clientAddPendResponses(fd, packet) == -1) {
+				warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
+			} else {	
+				int datalen = (packet[6] << 8) | (packet[5]);
+				printf("sending %lf '", getFloat(packet, 7));
+				for(int i = 0; i < datalen + 8; ++i) {
+					printf(" 0x%x", (signed) packet[i]);
+				}
+				printf("'\n");
+				writeQuad((char *) packet, datalen +8);
+			}
+		}
+		
+		char * rest = &buffer[newline] + 1;
+		size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
+		/* Delete parsed data and move the rest to the left */
+		memmove(buffer, rest, restLen +1);
+	}
+}
+
+static void quad_recv() {
+	/* Check to see which command we are receiving. If it is one that needs to be passed on
+		onto  the clients, do so.
+	 */
+	
+	int validPacket;
+	unsigned char *data;
+	metadata_t metadata;
+	static unsigned char respBuf[2048];
+	static size_t respBufLen;
+
+	/**
+	 * Read the response from the control loop
+	 */
+	int respLen = readQuad((char *) respBuf + respBufLen,
+			CMD_MAX_LENGTH);
+	if(respLen <= 0) {
+		perror("ERROR reading from quad...\n");
+		return;
+	}
+	respBufLen += respLen;
+
+	if (respBufLen < 8) {
+		/* not long enough yet */
+		printf("too short!!\n");
+		return;
+	}
+
+	// Validate the message is correctly formatted
+	validPacket = parse_packet((unsigned char *) respBuf, &data, &metadata);
+	if (validPacket == -1) {
+		warnx("Doesn't have start byte.");
+		/* nuke packet */
+		respBufLen = 0;
+		return;
+	}
+
+	/* Get datalen */
+	size_t datalen = metadata.data_len;
+	if (datalen > CMD_MAX_LENGTH - 8) {
+		/* Very invalid packet. Nuke that shit */
+		respBufLen = 0;
+		return;
+	}
+	
+	if (respBufLen < datalen + 8) {
+		/* Packet not yet fully read */
+		return;
+	}
+
+	if (validPacket == 0) {
+		/* At least enough data read to check checksum, and it was good!*/
+		char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText;
+		float value = getFloat((unsigned char *)respBuf, 7);
+		//printf("Quad : %s, %lf\n", cmdText, value);
+		/* 
+		   Assuming the quad sends the correct info.. This hasn't been tested yet due to a lack of
+		   quad software. We can check how to format by the cmdText and pass to every client.
+		   */
+		char buffer[1048];
+		sprintf(buffer, "%s %lf\n", cmdText, value);
+
+		for(int fd = 0; fd <= max_fd; ++fd) {
+			if (get_client_index(fd) > -1) {
+				write(fd, buffer, datalen + 8);
+			}
+		}
+	} else {
+		warnx("Checksum mismatch!");
+	}
+	memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
+	respBufLen -= datalen + 8;
+}
+
+static int wasDisconnected(int fd) {
+	char buff;
+	if(recv(fd, &buff, 1, MSG_PEEK | MSG_DONTWAIT) == 0)
+	{
+		remove_client(fd);
+		safe_fd_clr(fd, &rfds_master, &max_fd);
+		printf("fd %d has disconnect and was removed\n", fd);
+		return 1;
+	}
+	return 0;
+}
+
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y) {
+  /* Perform the carry for the later subtraction by updating y. */
+  if (x->tv_usec < y->tv_usec) {
+    int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
+    y->tv_usec -= 1000000 * nsec;
+    y->tv_sec += nsec;
+  }
+  if (x->tv_usec - y->tv_usec > 1000000) {
+    int nsec = (x->tv_usec - y->tv_usec) / 1000000;
+    y->tv_usec += 1000000 * nsec;
+    y->tv_sec -= nsec;
+  }
+  /* Compute the time remaining to wait.
+     tv_usec is certainly positive. */
+  result->tv_sec = x->tv_sec - y->tv_sec;
+  result->tv_usec = x->tv_usec - y->tv_usec;
+  /* Return 1 if result is negative. */
+  return x->tv_sec < y->tv_sec;
+}
+
+void findTimeDiff(int respID) {
+	struct timeval result, tend;
+	gettimeofday(&tend, NULL);
+	timeval_subtract(&result, &tend, &timeArr[respID%MAX_HASH_SIZE]);
+	printf("(BackEnd): Elapsed time = %ld ms\n", result.tv_usec/1000);
+	// printf("print to log\n");
+	// char tmp[8];
+	// snprintf(tmp, 8, "%ld \tms\n", result.tv_usec/1000);
+	// writeStringToLog(tmp);
+}

groundStation/src/backend/callbacks.c

+#include "commands.h"
+
+/* New stuff - this is nice and clean */
+
+/* Override any callbacks here */
+
+
+/****** LEGACY CODE BE VERY AFRAID ********/
+
+// TAKE THESE OUT WHEN IMPLEMENTING ON THE QUAD SIDE
+float getFloat(unsigned char* str, int pos) {
+	union {
+		float f;
+		int i;
+	} x;
+	x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
+	return x.f;
+}
+
+int getInt(unsigned char* str, int pos) {
+	int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
+	return i;
+}
+//------------------------------------------------
+
+int cb_debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("function for debug\n");
+	return 0;
+}
+
+int cb_update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	unsigned char update[28];
+	memcpy(update, ((float *)packet), 28);
+
+	int packetId = getInt(update, 0);
+	float y_pos = getFloat(update, 4);
+	float x_pos = getFloat(update, 8);
+	float alt_pos = getFloat(update, 12);
+	float roll = getFloat(update, 16);
+	float pitch = getFloat(update, 20);
+	float yaw = getFloat(update, 24);
+
+	structs->log_struct.currentQuadPosition.packetId = packetId;
+	structs->log_struct.currentQuadPosition.y_pos = y_pos;
+	structs->log_struct.currentQuadPosition.x_pos = x_pos;
+	structs->log_struct.currentQuadPosition.alt_pos = alt_pos;
+	structs->log_struct.currentQuadPosition.roll = roll;
+	structs->log_struct.currentQuadPosition.pitch = pitch;
+	structs->log_struct.currentQuadPosition.yaw = yaw;
+
+	printf("QUAD: VRPN Packet:");
+	printf("Packet ID: %d\n", packetId);
+	printf("Y Position: %f\n", y_pos);
+	printf("X Position: %f\n", x_pos);
+	printf("Altitude Position: %f\n", alt_pos);
+	printf("Roll: %f\n", roll);
+	printf("Pitch: %f\n", pitch);
+	printf("Yaw: %f\n", yaw);
+
+	printf("function for update\n");
+	return 0;
+}
+
+// Why is this here?
+// This should be on the ground station side
+int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("Logging: %s\n", packet);
+	return 0;
+}
+
+int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("This is the response: %s\n", packet);
+
+	return 0;
+}
+
+// ------------------------------------------------------------------
+
+int setyaw(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+
+	printf("%f\n", value);
+
+	structs->setpoint_struct.desiredQuadPosition.yaw = value;
+
+	printf("function for setyaw: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+
+	return 0;
+}
+
+int setyawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.yaw_angle_pid.Kp = value;
+
+	printf("function for setyawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
+
+	return 0;
+}
+
+int setyawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.yaw_angle_pid.Kd = value;
+
+	printf("function for setyawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
+
+	return 0;
+}
+
+int setroll(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.roll = value;
+
+	printf("function for setroll: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
+
+	return 0;
+}
+
+int setrollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.local_y_pid.Kp = value;
+
+	printf("function for setrollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
+
+	return 0;
+}
+
+int setrolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.local_y_pid.Kd = value;
+
+	printf("function for setrolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
+
+	return 0;
+}
+
+int setpitch(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.pitch = value;
+
+	printf("function for setpitch: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
+
+	return 0;
+}
+
+int setpitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.local_x_pid.Kp = value;
+
+	printf("function for setpitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
+
+	return 0;
+}
+
+int setpitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.local_x_pid.Kd = value;
+
+	printf("function for setpitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
+
+	return 0;
+}
+
+// ------------------------------------------------------------
+// These should be renamed to altitude!
+int setthrottle(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
+
+	printf("function for setthrottle: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
+
+	return 0;
+}
+
+int setthrottlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Kp = value;
+
+	printf("function for setthrottlep: %f\n", structs->parameter_struct.alt_pid.Kp);
+
+	return 0;
+}
+
+int setthrottlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Ki = value;
+
+	printf("function for setthrottlei: %f\n", structs->parameter_struct.alt_pid.Ki);
+
+	return 0;
+}
+
+int setthrottled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Kd = value;
+
+	printf("function for setthrottled: %f\n", structs->parameter_struct.alt_pid.Kd);
+
+	return 0;
+}
+int getyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
+	return 0;
+}
+int getroll(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getrollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getrolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getpitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+
+// These should be renamed to altitude!
+// ------------------------------------------------------------
+
+
+
+
+int getgyro(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	printf("function for getgyro\n");
+	return 0;
+}
+
+int getpitchangle(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for getpitchangle\n");
+	return 0;
+}
+
+int getrollangle(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for getrollangle\n");
+	return 0;
+}
+
+
+int getaccel(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for getaccel\n");
+	return 0;
+}
+
+
+int respgyro(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for respgyro\n");
+	return 0;
+}
+
+int resppitchangle(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	printf("function for resppitchangle\n");
+	return 0;
+}
+
+int resprollangle(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for resprollangle\n");
+	return 0;
+}
+
+
+int respaccel(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for respaccel\n");
+	return 0;
+}
+
+int respyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
+	return 0;
+}
+int resproll(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resprollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resprolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resppitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resppitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resppitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}

groundStation/src/backend/callbacks.h

+#ifndef __callbacks_h
+#define __callbacks_h
+
+/* Grab some stupid stuff from legacy code */
+#include "type_def.h"
+
+/* Make commands.c happy */
+typedef void (command_cb)(unsigned char *command, 
+		int dataLen, modular_structs_t *structs);
+
+#endif

groundStation/src/backend/cb_default.h

+#include "commands.h"
+
+/* The cb_default used on the groundStation. This file MUST NOT BE INCLUDED
+ * by anything except for commands.c */
+
+/* cb_default used by portable commands.c */
+int cb_default(unsigned char * command, int dataLen, modular_structs_t *structs)
+{
+	    return 0;
+}

groundStation/src/backend/commands.c

+#include "commands.h"
+
+/* This file defines the commands structure.
+ * This is the canonical reference for all commands
+ * used. This file can be used - unchanged - on both the quad side
+ * and on the ground station side. The commands.h file is also entirely
+ * portable and may be used unchanged.
+ *
+ * This file (commands.c) and the matching header (commands.h)
+ * are fully portable (quad + groundStation).
+ * 
+ * To use this file, three non-portable files must also exist:
+ *  - callbacks.h. Typedef for command_cb
+ *  - cb_default.h. Implementation of cb_default.
+ *  - callbacks.c file. Contains implemented callbacks.
+ *
+ * There are two mandatory things that must be implemented in
+ * other files for this to work: First, in callbacks.h, create a typedef
+ * for command_cb. See the callbacks.h of the ground station for an
+ * example.
+ *
+ * Second, in cb_default.h, implement the function
+ * cb_default. This function should do nothing; it will be the 
+ * default action for an unimplemented callback. Note that because
+ * the function is implemented in the .h file, cb_default.h MUST NOT
+ * be included in any other file!
+ *
+ * To implement callbacks, simply define them in callbacks.c.
+ *
+ */
+
+/* List of callbacks. DO NOT MODIFY THESE IN THIS FILE - 
+ * Simply implement a function with the same name
+ * in a different file (callbacks.c) and these will
+ * be overridden.
+ */
+
+/* Grab the default callback configuration */
+#include "cb_default.h"
+
+command_cb cb_debug __attribute__((weak, alias("cb_default")));
+command_cb cb_setyawp __attribute__((weak, alias("cb_default")));
+
+/* Command structure */
+struct MessageType MessageTypes[MAX_TYPE] =
+{
+	// DEBUG
+	{
+		// Message Type ID
+		0x00,
+
+		// Debug Subtypes
+		{
+			// NONE subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"debug",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				&cb_debug
+			}
+		}
+	},
+
+	// CALIBRATION
+	{
+		// Message Type ID
+		0x01,
+
+		// Calibration Subtypes (PID coefficients)
+		{
+			// yaw p constant subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"setyawp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// yaw i constant subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"setyawi",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// yaw d constant subtype
+			{
+				// ID
+				0x02,
+				// Command text
+				"setyawd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// roll p constant subtype
+			{
+				// ID
+				0x03,
+				// Command text
+				"setrollp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// roll i constant subtype
+			{
+				// ID
+				0x04,
+				// Command text
+				"setrolli",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// roll d constant subtype
+			{
+				// ID
+				0x05,
+				// Command text
+				"setrolld",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// pitch p constant subtype
+			{
+				// ID
+				0x06,
+				// Command text
+				"setpitchp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// pitch i constant subtype
+			{
+				// ID
+				0x07,
+				// Command text
+				"setpitchi",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// pitch d constant subtype
+			{
+				// ID
+				0x08,
+				// Command text
+				"setpitchd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// yawrate p constant subtype
+			{
+				// ID
+				0x09,
+				// Command text
+				"setyawratep",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// yawrate i constant subtype
+			{
+				// ID
+				0x0A,
+				// Command text
+				"setyawratei",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// yawrate d constant subtype
+			{
+				// ID
+				0x0B,
+				// Command text
+				"setyawrated",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// rollrate p constant subtype
+			{
+				// ID
+				0x0C,
+				// Command text
+				"setrollratep",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// rollrate i constant subtype
+			{
+				// ID
+				0x0D,
+				// Command text
+				"setrollratei",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// rollrate d constant subtype
+			{
+				// ID
+				0x0E,
+				// Command text
+				"setrollrated",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// pitchrate p constant subtype
+			{
+				// ID
+				0x0F,
+				// Command text
+				"setpitchratep",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// pitchrate i constant subtype
+			{
+				// ID
+				0x10,
+				// Command text
+				"setpitchratei",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// pitchrate d constant subtype
+			{
+				// ID
+				0x11,
+				// Command text
+				"setpitchrated",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// height p constant subtype
+			{
+				// ID
+				0x12,
+				// Command text
+				"setheightp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// height i constant subtype
+			{
+				// ID
+				0x13,
+				// Command text
+				"setheighti",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// height d constant subtype
+			{
+				// ID
+				0x14,
+				// Command text
+				"setheightd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// lat p constant subtype
+			{
+				// ID
+				0x15,
+				// Command text
+				"setlatp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// lat i constant subtype
+			{
+				// ID
+				0x16,
+				// Command text
+				"setlati",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// lat d constant subtype
+			{
+				// ID
+				0x17,
+				// Command text
+				"setlatd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// long p constant subtype
+			{
+				// ID
+				0x18,
+				// Command text
+				"setlongp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// long i constant subtype
+			{
+				// ID
+				0x19,
+				// Command text
+				"setlongi",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// long d constant subtype
+			{
+				// ID
+				0x1A,
+				// Command text
+				"setlongd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			}
+		}
+	},
+
+	// REQUEST
+	{
+		// Message Type ID
+		0x02,
+
+		// Request Subtypes
+		{
+			// accelerometer subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"getaccel",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// gyroscope subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"getgyro",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// pitch angle subtype
+			{
+				// ID
+				0x02,
+				// Command text
+				"getpitchangle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// roll angle subtype
+			{
+				// ID
+				0x03,
+				// Command text
+				"getrollangle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get yaw setpoint subtype
+			{
+				// ID
+				0x04,
+				// Command text
+				"getyaw",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get yaw p constant subtype
+			{
+				// ID
+				0x05,
+				// Command text
+				"getyawp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get yaw d constant subtype
+			{
+				// ID
+				0x06,
+				// Command text
+				"getyawd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get roll setpoint subtype
+			{
+				// ID
+				0x07,
+				// Command text
+				"getroll",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get roll p constant subtype
+			{
+				// ID
+				0x08,
+				// Command text
+				"getrollp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// getroll d constant subtype
+			{
+				// ID
+				0x09,
+				// Command text
+				"getrolld",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get pitch setpoint subtype
+			{
+				// ID
+				0x0A,
+				// Command text
+				"getpitch",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get pitch p constant subtype
+			{
+				// ID
+				0x0B,
+				// Command text
+				"getpitchp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get pitch d constant subtype
+			{
+				// ID
+				0x0C,
+				// Command text
+				"getpitchd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get throttle setpoint subtype
+			{
+				// ID
+				0x0D,
+				// Command text
+				"getthrottle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get throttle p constant subtype
+			{
+				// ID
+				0x0E,
+				// Command text
+				"getthrottlep",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get throttle i constant subtype
+			{
+				// ID
+				0x0F,
+				// Command text
+				"getthrottlei",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// get throttle d constant subtype
+			{
+				// ID
+				0x10,
+				// Command text
+				"getthrottled",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			}
+		}
+	},
+
+	// RESPONSE
+	{
+		// Message Type ID
+		0x03,
+
+		// Response Subtypes
+		{
+			// accelerometer subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"respaccel",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// gyroscope subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"respgyro",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// pitch angle subtype
+			{
+				// ID
+				0x02,
+				// Command text
+				"resppitchangle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// roll angle subtype
+			{
+				// ID
+				0x03,
+				// Command text
+				"resprollangle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp yaw setpoint subtype
+			{
+				// ID
+				0x04,
+				// Command text
+				"respyaw",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp yaw p constant subtype
+			{
+				// ID
+				0x05,
+				// Command text
+				"respyawp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp yaw d constant subtype
+			{
+				// ID
+				0x06,
+				// Command text
+				"respyawd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp roll setpoint subtype
+			{
+				// ID
+				0x07,
+				// Command text
+				"resproll",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp roll p constant subtype
+			{
+				// ID
+				0x08,
+				// Command text
+				"resprollp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resproll d constant subtype
+			{
+				// ID
+				0x09,
+				// Command text
+				"resprolld",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp pitch setpoint subtype
+			{
+				// ID
+				0x0A,
+				// Command text
+				"resppitch",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp pitch p constant subtype
+			{
+				// ID
+				0x0B,
+				// Command text
+				"resppitchp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp pitch d constant subtype
+			{
+				// ID
+				0x0C,
+				// Command text
+				"resppitchd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp throttle setpoint subtype
+			{
+				// ID
+				0x0D,
+				// Command text
+				"respthrottle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp throttle p constant subtype
+			{
+				// ID
+				0x0E,
+				// Command text
+				"respthrottlep",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp throttle i constant subtype
+			{
+				// ID
+				0x0F,
+				// Command text
+				"respthrottlei",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			},
+			// resp throttle d constant subtype
+			{
+				// ID
+				0x10,
+				// Command text
+				"respthrottled",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				NULL
+			}
+		}
+	},
+
+	// UPDATE
+	{
+		// Message Type ID
+		0x04,
+
+		// Update Subtypes
+		{
+			// NONE subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"update",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				NULL
+			}
+		}
+	},
+
+	// LOG
+	{
+		// Message Type ID
+		0x05,
+
+		// Log Subtypes
+		{
+			// NONE subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"log",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				NULL
+			},
+			// Response subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"response",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				NULL
+			}
+		}
+	},
+
+};

groundStation/src/backend/commands.h

+#ifndef _COMMANDS_H
+#define _COMMANDS_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "callbacks.h"
+
+// ----------------------
+// Helper stuff
+
+#define MAX_TYPE 6
+#define MAX_SUBTYPE 100
+
+enum Message{
+	BEGIN_CHAR = 0xBE,
+	END_CHAR   = 0xED
+};
+
+// This should also have double to avoid confusion with float values.
+enum DataType
+{
+	floatType,
+	intType,
+	stringType
+};
+
+// MESSAGE SUBTYPES
+struct MessageSubtype{
+	char ID;
+	char cmdText[100];
+	char cmdDataType;
+	command_cb * functionPtr;
+};
+
+// MESSAGE TYPES
+struct MessageType{
+	char ID;
+	struct MessageSubtype subtypes[MAX_SUBTYPE];
+};
+
+/* Defined in commands.c */
+extern struct MessageType MessageTypes[MAX_TYPE];
+
+/* Legacy functions - no idea what uses these. Please do not delete. */
+float getFloat(unsigned char* str, int pos);
+int getInt(unsigned char* str, int pos);
+/* end legacy crap */
+
+#endif

src/communication.c → groundStation/src/backend/communication.c

@@ -52,84 +52,95 @@ int checkInt(char *intString, int *value) {
 //--------------------------------
 
 // Formatting commands from ground station CLI
-int formatCommand(unsigned char *command, unsigned char **formattedCommand) {
-	//command[strlen((char *)command) - 1] = 0;
-	
-	tokenList_t tokens = tokenize((char *)command);
+int formatCommand(char *command, unsigned char **formattedCommand) {
+	//fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
+	char cmd[strlen(command)];
+	strncpy(cmd, command, strlen(command));
+	cmd[strlen(command)] = '\0';
+
+	tokenList_t tokens = tokenize(cmd);
 	float floatValue = 0.0;
 	int intValue = 0;
 	int valid;
-	metadata_t metadata = {};
+	metadata_t metadata;
 	
 	// ----------------------------------------------
-	if(tokens.numTokens > 1) {
-		for(int cmdIndex = 0; cmdIndex < NUM_COMMANDS; ++cmdIndex)
+	if(tokens.numTokens > 0) {
+		for(int type = 0; type < MAX_TYPE; type++)
 		{
-			if(strcmp(tokens.tokens[0], registeredCommands[cmdIndex].commandText) == 0)
+			for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
 			{
-
-				switch (registeredCommands[cmdIndex].dataType)
+				if(strcmp(tokens.tokens[0], MessageTypes[type].subtypes[subtype].cmdText) == 0)
 				{
-					// Validate the float input
-					case floatType:
-						valid = checkFloat(tokens.tokens[1], &floatValue);
-						if(!valid) {
-							return -1;
-						}
-						
-						printf("%f, %s\n", floatValue, tokens.tokens[1]);
-						
-						metadata.begin_char  = BEGIN_CHAR;
-						metadata.msg_type    = registeredCommands[cmdIndex].ID;
-						metadata.msg_subtype = registeredCommands[cmdIndex].subID;
-						metadata.msg_id      = msgNum++;
-						metadata.data_len    = sizeof(floatValue);
-						
-						formatPacket(&metadata, &floatValue, formattedCommand);
-						
-						break;
+					printf("Sending\n\ttype: %d, \n\tsubtype: %d\n\tcommand: %s\n", type, subtype, MessageTypes[type].subtypes[subtype].cmdText);
 					
-					// Validate the integer input
-					case intType:
-						valid = checkInt(tokens.tokens[1], &intValue);
-						if(!valid) {
-							return -1;
-						}
-						
-						metadata.begin_char  = BEGIN_CHAR;
-						metadata.msg_type    = registeredCommands[cmdIndex].ID;
-						metadata.msg_subtype = registeredCommands[cmdIndex].subID;
-						metadata.msg_id      = msgNum++;
-						metadata.data_len    = sizeof(intValue);
-						
-						formatPacket(&metadata, &intValue, formattedCommand);
-						
-						break;
-					
-					// Validate the string input (doesn't need to happen)
-					case stringType:
-						metadata.begin_char  = BEGIN_CHAR;
-						metadata.msg_type    = registeredCommands[cmdIndex].ID;
-						metadata.msg_subtype = registeredCommands[cmdIndex].subID;
-						metadata.msg_id      = msgNum++;
-						metadata.data_len    = strlen(tokens.tokens[1]);
+					// Make sure the second token is the right type
+					switch (MessageTypes[type].subtypes[subtype].cmdDataType)
+					{
+						// Validate the float input
+						case floatType:
+							metadata.begin_char = (char) BEGIN_CHAR;
+							metadata.msg_type = MessageTypes[type].ID;
+							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+
+							if(MessageTypes[type].ID == 0x01) {
+								valid = checkFloat(tokens.tokens[1], &floatValue);
+								if(!valid) {
+									return -1;
+								}
+								metadata.data_len = sizeof(floatValue);
+							} else {
+								metadata.data_len = 0;
+							}
+							
+							metadata.msg_id = msgNum++;
+							formatPacket(&metadata, &floatValue, formattedCommand);
+							break;
 						
-						formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
+						// Validate the integer input
+						case intType:
+							metadata.begin_char = (char) BEGIN_CHAR;
+							metadata.msg_type = MessageTypes[type].ID;
+							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+							
+							if(MessageTypes[type].ID == 0x01) {
+								valid = checkInt(tokens.tokens[1], &intValue);
+								if(!valid) {
+									return -1;
+								}
+								metadata.data_len = sizeof(intValue);
+							} else {
+								metadata.data_len = 0;
+							}
+							
+							metadata.msg_id = msgNum++;
+							formatPacket(&metadata, &intValue, formattedCommand);
+							
+							break;
 						
-						break;
-					default:
-						return -1;
+						// Validate the string input (doesn't need to happen)
+						case stringType:
+							metadata.begin_char = (char) BEGIN_CHAR;
+							metadata.msg_type = MessageTypes[type].ID;
+							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+							metadata.msg_id = msgNum++;
+							metadata.data_len = strlen(tokens.tokens[1]);
+							
+							formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
+							
+							break;
+						default:
+							return -1;
+					}
+					return 0;
 				}
-
-				return 0;
 			}
 		}
 	}
-	
+
 	// Only gets here if the command does not exist
-	return -1;
+	return -1;	
 }
-
 // QUAD & Ground Station
 // Format the log data from log_message
 //int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
@@ -154,7 +165,8 @@ int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedComm
 	(*formattedCommand)[2] = metadata->msg_subtype;
 	
 	//Msg id (msgNum is 2 bytes)
-	(*formattedCommand)[3] = metadata->msg_id;
+	(*formattedCommand)[3] = (metadata->msg_id & 0x000000ff);
+	(*formattedCommand)[4] = ((metadata->msg_id >> 8) & 0x000000ff);
 	
 	// Data length and data - bytes 5&6 for len, 7+ for data
 	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
@@ -224,15 +236,17 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met
 	}
 
 	// compare checksum
-	if(packet_checksum != calculated_checksum)
+	if(packet_checksum != calculated_checksum) {
 		fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
+		return -2;
+	}
 
 	return 0;
 }
 
 // QUAD & Ground Station
 // Process the command received
-int processCommand(unsigned char *packet, unsigned int cmdIndex) {
+int processCommand(unsigned char *packet, modular_structs_t *structs) {
 	int validPacket;
 	unsigned char *data;
 	metadata_t metadata;
@@ -244,9 +258,13 @@ int processCommand(unsigned char *packet, unsigned int cmdIndex) {
 	}
 	
 	if(metadata.data_len >= 0) {
-		// Call the appropriate subtype function
-		(* (registeredCommands[cmdIndex].functionPtr))(data, metadata.data_len);
-		
+		/* Null check*/
+		if (MessageTypes[(unsigned char)metadata.msg_type].subtypes[
+				(unsigned char) metadata.msg_subtype].functionPtr) {
+			// Call the appropriate subtype function
+			(* (MessageTypes[(unsigned char)metadata.msg_type].subtypes[(unsigned char)metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
+		}
+			
 		return 0;
 	}
 	

src/communication.h → groundStation/src/backend/communication.h

@@ -10,12 +10,10 @@
 tokenList_t tokenize(char* cmd);
 int checkFloat(char *floatString, float *value);
 int checkInt(char *intString, int *value);
-int formatCommand(unsigned char *command, unsigned char **formattedCommand);
+int formatCommand(char *command, unsigned char **formattedCommand);
 int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);
 int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data);
-int processCommand(unsigned char *command, unsigned int cmdIndex);
+int processCommand(unsigned char *command, modular_structs_t *structs);
 int logData(unsigned char *log_msg, unsigned char *formattedCommand);
-float getFloat(unsigned char* str, int pos);
-int getInt(unsigned char* str, int pos);
 
-#endif
+#endif
\ No newline at end of file

groundStation/src/backend/config.h

+#ifndef __CONFIG_H
+#define __CONFIG_H
+
+
+#define DEFAULT_SOCKET "/var/run/ucart.socket"
+#define SOCKET_ENV "UCART_SOCKET"
+#define NOQUAD_ENV "UCART_NO_QUAD"
+#define NOVRPN_ENV "UCART_NO_VRPN"
+
+
+// If you are planning on using any of these env vars and you have
+// 	exported them with normal user rights. You will need to run the 
+// 	backend with sudo elevation and with the --preserve-env flag or -E
+#define QUAD_WIFI_ENV "UCART_USE_WIFI"
+#define QUAD_IP_ENV "UCART_QUAD_IP"
+#define QUAD_IP_DEFAULT "192.168.4.1"
+#define QUAD_PORT_ENV "UCART_QUAD_PORT"
+#define QUAD_PORT_DEFAULT 8080
+
+#endif

src/logger.c → groundStation/src/backend/logger.c

@@ -4,21 +4,30 @@
  */
 #include "logger.h"
 #include <stdio.h>
+#include <err.h>
+#include <pthread.h>
 
-int quadlog_file;
+static FILE * quadlog_file = NULL;
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 
 int createLogFile(int argc, char* argv)
 {
-	char log_file[300] = {'l', 'o', 'g','s', '/'};
+	if (quadlog_file != NULL) {
+		return -1;
+	}
+
+	if (pthread_mutex_lock(&mutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	char log_file[300];
+	strcpy(log_file, "logs/");
 	if(argc >= 2)
 	{
-		strcat(log_file, argv);
+		strncat(log_file, argv, 294);
 		printf("Creating log file '%s'...\n",log_file);
-		quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666);
-		return quadlog_file;
-	}
-	else
-	{
+		quadlog_file = fopen(log_file, "a");
+	} else {
 		time_t rawtime;
 		char timestr [30];
 		time(&rawtime);
@@ -26,7 +35,7 @@ int createLogFile(int argc, char* argv)
 
 		// Lets convert space to _ in
 		char *p = timestr;
-		int i = 0;
+		size_t i = 0;
 		while(i < strlen(timestr))
 		{ 
 		    if (*p == ' ')
@@ -40,18 +49,60 @@ int createLogFile(int argc, char* argv)
 		strncat(log_file, timestr, strlen(timestr) -1 );
 		strcat(log_file, ".txt");
 		printf("Creating log file '%s'...\n",log_file);
-		quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666);
-		return quadlog_file;
+		quadlog_file = fopen(log_file, "a");
+	}
+	if (pthread_mutex_unlock(&mutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
 	}
+	return 0;
 }
 
 int updateLogFile(const struct ucart_vrpn_TrackerData * td)
 {
-	return dprintf(quadlog_file, "FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
+	int retval;
+
+	if (pthread_mutex_lock(&mutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+     
+	retval = fprintf(quadlog_file, 
+			"FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
 		td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
+
+	if (pthread_mutex_unlock(&mutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+	
+	return retval;
 }
 
 int writeStringToLog(const char * string)
 {
-	return dprintf(quadlog_file, "%s", string);
+	int retval;
+
+	if (pthread_mutex_lock(&mutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	retval = fprintf(quadlog_file, "%s", string);
+
+	if (pthread_mutex_unlock(&mutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	return retval;
+}
+
+void closeLogFile(void)
+{
+	if (pthread_mutex_lock(&mutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	fclose(quadlog_file);
+
+	if (pthread_mutex_unlock(&mutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+
 }

src/logger.h → groundStation/src/backend/logger.h

@@ -15,6 +15,7 @@
 int createLogFile(int, char*);
 int writeStringToLog(const char*);
 int updateLogFile(const struct ucart_vrpn_TrackerData* );
+void closeLogFile();
 
 
-#endif
\ No newline at end of file
+#endif

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

@@ -23,13 +23,13 @@ extern "C"
 #endif
 	struct ucart_vrpn_tracker;
 	struct ucart_vrpn_TrackerData {
-		double x;
-		double y;
-		double z;
+		float x;
+		float y;
+		float z;
 
-		double pitch;
-		double roll;
-		double yaw;
+		float pitch;
+		float roll;
+		float yaw;
 
 		double fps;
 		struct timeval timestamp;
@@ -54,16 +54,19 @@ extern "C"
 namespace microcart
 {
 	struct TrackerData {
-		double x;
-		double y;
-		double z;
+	public:
+		float x;
+		float y;
+		float z;
 
-		double pitch;
-		double roll;
-		double yaw;
+		float pitch;
+		float roll;
+		float yaw;
 
 		double fps;
 		timeval timestamp;
+
+		TrackerData();
 	};
 
 	class Tracker {

groundStation/src/cli/README.md

+# CLI
+
+## Usage
+
+The Cli has been designed to function similarly to command line programs. Run the program using ./Cli
+
+To run a specific command in the 

groundStation/src/cli/cli.c

+#include <stdio.h>
+#include <string.h>
+#include <err.h>
+#include <libgen.h>
+#include <getopt.h>
+
+#include "cli.h"
+
+int main(int argc, char **argv)
+{
+	int cmdID = -1;
+	char * command;
+	int c;
+	int i , useSymlink = 0;
+	struct backend_conn  *conn;
+	int needCliHelp = 0, needCommandHelp = 0;
+
+	// Determine if the cli was called using a symlink
+	command = basename(argv[0]);
+	for(i = 0; i < MAX_COMMANDS; ++i) {
+		if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
+		{
+			cmdID = i;
+			useSymlink = 1;
+		}
+	}
+
+	// Verify the user has entered enough information to continue
+	if (argc < 2 && !useSymlink) {
+		printf("Incorrect usage :\n");
+		printf("Usage Syntax: \n\t./Cli command [options...]\n\n");
+		printf("For a list of available 'command' names\n\t./Cli --help\n\n");
+		printf("For a list of available options for a 'command'\n\t./Cli command --help\n");
+		return -1;
+	}
+
+	if (!useSymlink) {
+		// Determine if the user called for help on the cli
+		needCliHelp = (strncmp("--help", argv[1], strlen(argv[1])) ==  0);
+	}
+
+	// If the user runs './Cli help' , provide the user with a list of commands available.
+	if (needCliHelp) {
+		printf("Usage Syntax: \n\t./Cli command [options...]\n\n");
+		printf("Available 'command' names include the following\n");
+		for(int i = 0; i < MAX_COMMANDS; ++i) {
+			printf("\t '%s'\n",commandNames[i]);
+		}
+		return 0;
+	}
+
+	// recognize which cli command the user has entered
+	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 '%s' with a command. Please try again...\n", command);
+		printf("For help running the program, run ./Cli --help\n");
+		return -1;
+	}
+
+	// Determine if the user called for help on the command
+	if (!useSymlink && argc > 2) {
+		if (strncmp("--help", argv[2], strlen(argv[2])) == 0) {
+			needCommandHelp = 1;
+		}
+	} else if (useSymlink && argc > 1) {
+		if (strncmp("--help", argv[1], strlen(argv[1])) == 0) {
+			needCommandHelp = 1;
+		}
+	}
+	
+	/**
+	 * I the user has asked for help, and we have already found
+	 *  the command that they are trying to use. Then we need
+	 *  to be able to provide the help info wihtout the 
+	 *  requirement of the backend
+	 *
+	 * It is important to note that this will only work if the 
+	 * 	command the user has asked for handles this help case 
+	 * 
+	 */
+
+	// Only require the backend if we will need to use it.
+	if (!needCommandHelp) {
+		// Create the connection to the backend
+		conn = ucart_backendConnect();
+		if (conn == NULL) {
+			return -1;
+		}
+	}
+
+	// Call the appropriate function
+	if (useSymlink) {
+		(*cli_functions[cmdID]) (conn, argc, &argv[0]);
+	}else {
+		(*cli_functions[cmdID]) (conn, argc-1, &argv[1]);
+	}
+
+	// Disconnect from the backend
+	if (!needCommandHelp) {
+		ucart_backendDisconnect(conn);
+	}
+    return 0;
+}
\ No newline at end of file

groundStation/src/cli/cli.h

+#ifndef __CLI_H
+#define __CLI_H
+
+#include "cli_setsetpoint.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,
+	CMD_SETSETPOINT,
+	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,
+	&cli_setsetpoint
+};
+
+static char* commandNames[MAX_COMMANDS] = {
+	"monitor",
+	"getpid",
+	"setpid",
+	"getimu",
+	"setsetpoint"
+};
+
+#endif

groundStation/src/cli/cli_getimu.c

+#include <stdio.h>
+
+#include "cli_getimu.h"
+
+int cli_getimu(struct backend_conn * conn, int argc, char ** argv) {
+	printf("This functionality has not been added yet\n");
+	return 0;
+}

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, int argc, char ** argv);
+
+#endif
\ No newline at end of file