Merge branch 'groundStation'

.gitignore

+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+
+
+# vrpn/build files
+src/vrpn/build*
+src/vrpn/pc_linux64/*
+
+#Exacutables
+BlueTooth
+logs

groundStation/Makefile

+# Declaration of variables
+GCC=gcc
+GXX=g++
+CFLAGS= -Wall  -std=c99 -g
+CPPFLAGS= -Wall  -std=c++11 -g
+INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
+
+# Directories
+SRCDIR=src 
+INCDIR=inc src src/vrpn src/vrpn/quat src/vrpn/build 
+OBJDIR=obj
+
+# Final exacutable name
+EXE=BlueTooth
+
+# File names
+CSOURCES := $(foreach dir, $(SRCDIR), $(wildcard $(dir)/*.c )) 
+CPPSOURCES := $(foreach dir, $(SRCDIR), $(wildcard $(dir)/*.cpp ))
+COBJECTS = $(CSOURCES:.c=.o) 
+CPPOBJECTS = $(CPPSOURCES:.cpp=.o)
+OBJECTS = $(CPPOBJECTS) $(COBJECTS) 
+
+LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat 
+
+# Default target
+all: logs $(EXE)
+
+vrpn: vrpn/build
+
+# Main target
+$(EXE): $(OBJECTS) 
+	$(GXX) $(CPPFLAGS) $^ -o $@  $(INCLUDES) $(LIBS)
+
+$(COBJECTS) : %.o : %.c
+	$(GCC)  $(CFLAGS) -c $< -o $@ $(INCLUDES) $(LIBS)
+
+$(CPPOBJECTS) : %.o : %.cpp
+	$(GCC)  $(CPPFLAGS) -c $< -o $@ $(INCLUDES) $(LIBS)
+
+vrpn/build:
+	mkdir -p src/vrpn/build
+	cd src/vrpn/build && cmake .. && make
+
+logs:
+	mkdir -p logs
+
+clean_logs:
+	rm -f logs/*
+
+clean:
+	rm -f $(EXE) $(OBJECTS)
+
+debug:
+	@echo $(COBJECTS)
+	@echo $(CPPOBJECTS)

groundStation/README.md

+# groundStation
\ No newline at end of file

groundStation/src/commands.c

+#include "commands.h"
+
+static command_t registeredCommands[NUM_COMMANDS] = {
+	{ 0x00, 0x00, stringType, "debug", 			&debug },				//DEBUG
+	{ 0x01, 0x00, stringType, "setyaw", 		&setyaw },				//CALIBRATION
+	{ 0x01, 0x01, stringType, "setyawp", 		&setyawp },
+	{ 0x01, 0x02, stringType, "setyawd", 		&setyawd },
+	{ 0x01, 0x03, stringType, "setroll", 		&setroll },
+	{ 0x01, 0x04, stringType, "setrollp", 		&setrollp },
+	{ 0x01, 0x05, stringType, "setrolld", 		&setrolld },
+	{ 0x01, 0x06, stringType, "setpitch", 		&setpitch },
+	{ 0x01, 0x07, stringType, "setpitchp",		&setpitchp },
+	{ 0x01, 0x08, stringType, "setpitchd", 		&setpitchd },
+	{ 0x01, 0x09, stringType, "setthrottle", 	&setthrottle },
+	{ 0x01, 0x0A, stringType, "setthrottlep", 	&setthrottlep },
+	{ 0x01, 0x0B, stringType, "setthrottlei", 	&setthrottlei },
+	{ 0x01, 0x0C, stringType, "setthrottled", 	&setthrottled },
+	{ 0x02, 0x00, stringType, "getaccel", 		&getaccel },			//REQUEST
+	{ 0x02, 0x01, stringType, "getgyro", 		&getgyro },
+	{ 0x02, 0x02, stringType, "getpitchangle", 	&getpitchangle },
+	{ 0x02, 0x03, stringType, "getrollangle", 	&getrollangle },
+	{ 0x03, 0x00, stringType, "accelresp",		accelresp },			//RESPONSE
+	{ 0x03, 0x01, stringType, "gyroresp",   	&gyroresp },
+	{ 0x03, 0x02, stringType, "pitchangleresp", &pitchangleresp },
+	{ 0x03, 0x03, stringType, "rollangleresp", 	&rollangleresp },
+	{ 0x04, 0x00, stringType, "update", 		&update },				//UPDATE
+	{ 0x04, 0x01, stringType, "beginupdate", 	&beginupdate },
+	{ 0x05, 0x00, stringType, "log", 			&logdata },				//LOG
+	{ 0x05, 0x01, stringType, "response", 		&response },
+};
+
+int debug(unsigned char *c, int dataLen){
+	return 0;
+}
+int setyaw(unsigned char *c, int dataLen){
+	return 0;
+}
+int setyawp(unsigned char *c, int dataLen){
+	return 0;
+}
+int setyawd(unsigned char *c, int dataLen){
+	return 0;
+}
+int setroll(unsigned char *c, int dataLen){
+	return 0;
+}
+int setrollp(unsigned char *c, int dataLen){
+	return 0;
+}
+int setrolld(unsigned char *c, int dataLen){
+	return 0;
+}
+int setpitch(unsigned char *c, int dataLen){
+	return 0;
+}
+int setpitchp(unsigned char *c, int dataLen){
+	return 0;
+}
+int setpitchd(unsigned char *c, int dataLen){
+	return 0;
+}
+int setthrottle(unsigned char *c, int dataLen){
+	return 0;
+}
+int setthrottlep(unsigned char *c, int dataLen){
+	return 0;
+}
+int setthrottlei(unsigned char *c, int dataLen){
+	return 0;
+}
+int setthrottled(unsigned char *c, int dataLen){
+	return 0;
+}
+int getaccel(unsigned char *c, int dataLen){
+	return 0;
+}
+int getgyro(unsigned char *c, int dataLen){
+	return 0;
+}
+int getpitchangle(unsigned char *c, int dataLen){
+	return 0;
+}
+int getrollangle(unsigned char *c, int dataLen){
+	return 0;
+}
+int accelresp(unsigned char *c, int dataLen){
+	return 0;
+}
+int gyroresp(unsigned char *c, int dataLen){
+	return 0;
+}
+int pitchangleresp(unsigned char *c, int dataLen){
+	return 0;
+}
+int rollangleresp(unsigned char *c, int dataLen){
+	return 0;
+}
+int update(unsigned char *c, int dataLen){
+	return 0;
+}
+int beginupdate(unsigned char *c, int dataLen){
+	return 0;
+}
+int logdata(unsigned char *c, int dataLen){
+	return 0;
+}
+int response(unsigned char *packet, int dataLen){
+	return 0;
+}
\ No newline at end of file

groundStation/src/commands.h

+#ifndef _COMMANDS_H
+#define _COMMANDS_H
+
+#define NUM_COMMANDS 26
+
+//TODO handle with enums
+#define MAX_TYPE 6
+#define MAX_SUBTYPE 100
+
+int debug(unsigned char *c, int dataLen);
+int setyaw(unsigned char *c, int dataLen);
+int setyawp(unsigned char *c, int dataLen);
+int setyawd(unsigned char *c, int dataLen);
+int setroll(unsigned char *c, int dataLen);
+int setrollp(unsigned char *c, int dataLen);
+int setrolld(unsigned char *c, int dataLen);
+int setpitch(unsigned char *c, int dataLen);
+int setpitchp(unsigned char *c, int dataLen);
+int setpitchd(unsigned char *c, int dataLen);
+int setthrottle(unsigned char *c, int dataLen);
+int setthrottlep(unsigned char *c, int dataLen);
+int setthrottlei(unsigned char *c, int dataLen);
+int setthrottled(unsigned char *c, int dataLen);
+int getaccel(unsigned char *c, int dataLen);
+int getgyro(unsigned char *c, int dataLen);
+int getpitchangle(unsigned char *c, int dataLen);
+int getrollangle(unsigned char *c, int dataLen);
+int accelresp(unsigned char *c, int dataLen);
+int gyroresp(unsigned char *c, int dataLen);
+int pitchangleresp(unsigned char *c, int dataLen);
+int rollangleresp(unsigned char *c, int dataLen);
+int update(unsigned char *c, int dataLen);
+int beginupdate(unsigned char *c, int dataLen);
+int logdata(unsigned char *c, int dataLen);
+int response(unsigned char *packet, int dataLen);
+
+enum Message{
+	BEGIN_CHAR = 0xBE,
+	END_CHAR   = 0xED
+};
+
+enum DataType
+{
+	floatType,
+	intType,
+	stringType
+};
+
+typedef struct command {
+	char ID;
+	char subID;
+	char dataType;
+	char commandText[100];
+	int (*functionPtr)(unsigned char *command, int dataLen);
+}command_t;
+
+enum CommandIDs{
+	DEBUG,
+	CALIBRATION,
+	REQUEST,
+	RESPONSE,
+	UPDATE,
+	LOG,
+	MAX_COMMAND_IDS
+};
+
+static command_t registeredCommands[NUM_COMMANDS];
+
+#endif
\ No newline at end of file

groundStation/src/communication.c

+#include "communication.h"
+#include "commands.h"
+#include <string.h>
+#include <ctype.h>
+
+static int msgNum = 0;
+
+tokenList_t tokenize(char* cmd) {
+	int maxTokens = 16;
+	tokenList_t ret;
+	ret.numTokens = 0;
+	ret.tokens = malloc(sizeof(char*) * maxTokens);
+	ret.tokens[0] = NULL;
+
+	int i = 0;
+	char* token = strtok(cmd, " ");
+	while (token != NULL && i < maxTokens - 1) {
+		ret.tokens[i++] = token;
+		ret.tokens[i] = NULL;
+		ret.numTokens++;
+		token = strtok(NULL, " ");
+	}
+
+	return ret;
+}
+
+int checkFloat(char *floatString, float *value) {
+	char *tmp;
+	*value = strtod(floatString, &tmp);
+	if(!(isspace(*tmp) || *tmp == 0)) {
+		fprintf(stderr, "%s is not a valid floating-point number\n", floatString);
+		return 0;
+	}
+	return 1;
+}
+
+int checkInt(char *intString, int *value) {
+	char *tmp;
+	long temp_long;
+	temp_long = strtol(intString, &tmp, 0); // base 10 number inputted
+	if(temp_long < INT_MIN || temp_long > INT_MAX ||  !(isspace(*tmp) || *tmp == 0)) {
+		fprintf(stderr, "%s is not a valid integer number\n", intString);
+		return 0;
+	}
+	printf("temp: %ld\n\n", temp_long);
+	*value = (int) temp_long;
+	return 1;
+}
+
+//--------------------------------
+// Ground Station
+//--------------------------------
+
+// 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);
+	float floatValue = 0.0;
+	int intValue = 0;
+	int valid;
+	metadata_t metadata = {};
+	
+	// ----------------------------------------------
+	if(tokens.numTokens > 1) {
+		for(int cmdIndex = 0; cmdIndex < NUM_COMMANDS; ++cmdIndex)
+		{
+			if(strcmp(tokens.tokens[0], registeredCommands[cmdIndex].commandText) == 0)
+			{
+				switch (registeredCommands[cmdIndex].dataType)
+				{
+					// 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;
+					
+					// 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]);
+						
+						formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
+						
+						break;
+					default:
+						return -1;
+				}
+
+				return 0;
+			}
+		}
+	}
+	
+	// Only gets here if the command does not exist
+	return -1;
+}
+
+// QUAD & Ground Station
+// Format the log data from log_message
+//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
+int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand)
+{
+	*formattedCommand = malloc(sizeof(unsigned char) * metadata->data_len + 8);
+	//----------------------------------------------------------------------------------------------
+	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
+	//---------------------------------------------------------------------------------------------|
+	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+	//-------------------------------------------------------------------------------------------- |
+	//	   bytes||	   1	|	  1	   |	  1		 |	  2	  |		   2		 | var	|	 1	   |
+	//----------------------------------------------------------------------------------------------
+	
+	// Begin Char:
+	(*formattedCommand)[0] = metadata->begin_char;
+
+	// Msg type:
+	(*formattedCommand)[1] = metadata->msg_type;
+	
+	// Msg subtype
+	(*formattedCommand)[2] = metadata->msg_subtype;
+	
+	//Msg id (msgNum is 2 bytes)
+	(*formattedCommand)[3] = metadata->msg_id;
+	
+	// Data length and data - bytes 5&6 for len, 7+ for data
+	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
+	(*formattedCommand)[6] = (metadata->data_len >> 8) & 0x000000ff;
+	
+	memcpy(&((*formattedCommand)[7]), data, metadata->data_len);
+	
+	// Checksum
+	// receive data and calculate checksum
+	int i;
+	char data_checksum;
+	for(i = 0; i < 7 + metadata->data_len; i++)
+	{
+		data_checksum ^= (*formattedCommand)[i];
+	}
+	
+	(*formattedCommand)[7 + metadata->data_len] = data_checksum;
+	
+	return 0;
+}
+
+// returns the length of the data in bytes (datalen from packet) and fills data
+// and metadata with the packet information
+// use as follows:
+//
+//		packet is the entire packet message (formatted) 
+//		data is an unallocated (char *) (pass it to this function as &data) 
+//		meta_data is a pointer to an instance of metadata_t
+//
+int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
+{
+	//----------------------------------------------------------------------------------------------
+	//     index||     0    |     1    |      2      |  3 & 4 |      5 & 6       |  7+  |   end    |
+	//---------------------------------------------------------------------------------------------|
+	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+	//-------------------------------------------------------------------------------------------- |
+	//     bytes||     1    |     1    |      1      |    2   |        2         | var  |    1     |
+	//----------------------------------------------------------------------------------------------	
+	
+	// first byte must be the begin char
+	if(packet[0] != 0xBE)
+		return -1;
+
+	// receive metadata
+	meta_data->begin_char = packet[0];
+	meta_data->msg_type = packet[1];
+	meta_data->msg_subtype = packet[2];
+	meta_data->msg_id = (packet[4] << 8) | (packet[3]);
+	meta_data->data_len = (packet[6] << 8) | (packet[5]);
+	unsigned char packet_checksum = packet[7+meta_data->data_len];
+	//fprintf(stderr, "datalen: %d\n", meta_data->data_len);
+	
+	int i;
+	
+	// receive data
+	*data = malloc(meta_data->data_len);
+	for(i = 0; i < meta_data->data_len; i++)
+	{
+		(*data)[i] = packet[7+i];
+	}
+
+	// calculate checksum
+	unsigned char calculated_checksum = 0;
+	for(i = 0; i < meta_data->data_len + 7; i++)
+	{
+		calculated_checksum ^= packet[i];
+	}
+
+	// compare checksum
+	if(packet_checksum != calculated_checksum)
+		fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
+
+	return 0;
+}
+
+// QUAD & Ground Station
+// Process the command received
+int processCommand(unsigned char *packet, unsigned int cmdIndex) {
+	int validPacket;
+	unsigned char *data;
+	metadata_t metadata;
+	
+	// Validate the message is correctly formatted
+	validPacket = parse_packet(packet, &data, &metadata);
+	if(validPacket != 0) {
+		return -1;
+	}
+	
+	if(metadata.data_len >= 0) {
+		// Call the appropriate subtype function
+		(* (registeredCommands[cmdIndex].functionPtr))(data, metadata.data_len);
+		
+		return 0;
+	}
+	
+	// Only gets here if there is an error
+	return -1;
+}
+
+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;
+}

groundStation/src/communication.h

+#ifndef _COMMUNICATION_H
+#define _COMMUNICATION_H
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <limits.h>
+#include "type_def.h"
+
+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 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 logData(unsigned char *log_msg, unsigned char *formattedCommand);
+float getFloat(unsigned char* str, int pos);
+int getInt(unsigned char* str, int pos);
+
+#endif

groundStation/src/logger.c

+/* Author: Kris Burney
+ *
+ * Logger file for holding functions pertaining to loging to a file.
+ */
+#include "logger.h"
+#include <stdio.h>
+#include <err.h>
+#include <pthread.h>
+
+static FILE * quadlog_file = NULL;
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+
+int createLogFile(int argc, char* argv)
+{
+	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)
+	{
+		strncat(log_file, argv, 294);
+		printf("Creating log file '%s'...\n",log_file);
+		quadlog_file = fopen(log_file, "a");
+	} else {
+		time_t rawtime;
+		char timestr [30];
+		time(&rawtime);
+		sprintf(timestr,"%s",ctime(&rawtime));
+
+		// Lets convert space to _ in
+		char *p = timestr;
+		size_t i = 0;
+		while(i < strlen(timestr))
+		{ 
+		    if (*p == ' ')
+		          *p = '_';
+			i++;
+			p++;
+		}
+
+		// timestr has a weird char at the end of it.
+		// we will not include it in our file name
+		strncat(log_file, timestr, strlen(timestr) -1 );
+		strcat(log_file, ".txt");
+		printf("Creating log file '%s'...\n",log_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)
+{
+	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)
+{
+	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__);
+	}
+
+}

groundStation/src/logger.h

+/* Author: Kris Burney
+ *
+ * Logger header file for holding info pertaining to loging to a file.
+ */
+#ifndef _LOGGER_H
+#define _LOGGER_H
+
+#include <fcntl.h>
+#include <time.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "vrpn_tracker.hpp"
+
+int createLogFile(int, char*);
+int writeStringToLog(const char*);
+int updateLogFile(const struct ucart_vrpn_TrackerData* );
+void closeLogFile();
+
+
+#endif

groundStation/src/microcart_cli.c

+/* Author: Kris Burney
+ *
+ * BlueTooth socket program for passing vrpn data to quad.
+ */
+//system includes
+#include <err.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <signal.h>
+#include <sys/socket.h>
+#include <sys/select.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/rfcomm.h>
+#include <pthread.h>
+#include <assert.h>
+
+//user created includes
+#include "communication.h"
+#include "commands.h"
+#include "vrpn_tracker.hpp"
+#include "type_def.h"
+#include "logger.h"
+
+#define QUAD_BT_ADDR  "00:06:66:64:61:D6"
+#define QUAD_BT_CHANNEL  0x01
+#define CMD_MAX_LENGTH 1024
+
+// function prototypes
+void killHandler(int);
+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();
+// void *handleQuadResponse();
+// void *handleCliInput();
+// int atomic_check(int*, pthread_mutex_t*);
+void performCommand(char *, char *);
+int startsWith(const char *, const char *);
+int safe_fd_set(int , fd_set* , int* );
+int safe_fd_clr(int , fd_set* , int* );
+
+static void cb(struct ucart_vrpn_TrackerData *);
+
+/* Thread-safe wrappers */
+pthread_mutex_t quadSocketMutex;
+static ssize_t writeQuad(const char * buf, size_t count);
+
+// global variables
+static volatile int keepRunning = 1;
+const char *TRACKER_IP = "UAV@192.168.0.120:3883";
+static int zyboSocket;
+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";
+
+pthread_mutex_t quadResponseMutex, cliInputMutex ;
+unsigned char *respBuf, *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++;
+	// This will print the vrpn data to the terminal if necissary.
+	// Commented out because the callback will cover quad log data
+	//   at the end of flight.
+	/**if(!(count % 100)) {
+	 	printVrpnData(td);
+	 	printf("[Info] Received %d tracker updates.\n", count);
+	}**/
+}
+
+
+int main(int argc, char **argv)
+{
+	// pthread_t quadResponse, cliInput;
+	fd_set rfds;
+	int activity;
+	int max_fd = 0;
+	
+	signal(SIGINT, killHandler);
+
+	if (pthread_mutex_lock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	// if ((zyboSocket = connectToZybo()) < 0)
+	// {
+	// 	perror("Error connecting to Zybo...");
+	// 	free(respBuf);
+	// 	free(commandBuf);
+	// 	exit(1);
+	// }
+
+	if (pthread_mutex_unlock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+	
+	// create vrpnTracker instance
+	//tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
+
+	// open the log file
+	if(createLogFile(argc, argv[1]))
+	{
+		perror("Error creating log file...");
+		exit(1);
+	}
+//	writeStringToLog(logHeader);
+
+
+	// clear the fd set
+	FD_ZERO(&rfds);
+	// watch for input from stdin (fd 0) to see when it has input
+	safe_fd_set(fileno(stdin), &rfds, &max_fd);
+	// watch for input from the zybo socket
+	//safe_fd_set(zyboSocket, &rfds, &max_fd);
+
+	//printf("zyboSocket = %d, max_fd = %d\n", zyboSocket, max_fd);
+
+	//tell the quad we are ready to send it vrpn data
+	printf("sending Start Packet...\n");
+	//sendStartPacket();
+
+
+	// this function will be called whenever tracker receives data
+	// ucart_vrpn_tracker_addCallback(tracker, cb);
+
+	// start the prompt
+	fprintf(stdout, "$microcart> ");
+	while(keepRunning)
+	{
+		activity = select(max_fd+1, &rfds, NULL, NULL, NULL);
+		if(activity == -1) {
+			perror("select() ");
+		} else if (activity) {
+			for(unsigned int fd = 0; fd <= max_fd; ++fd) {
+				if (FD_ISSET(fd, &rfds)) {
+					if (fd == fileno(stdin)) {
+						unsigned char userCommand[CMD_MAX_LENGTH] = {};
+						read(fileno(stdin), (char *)userCommand, sizeof(userCommand));
+						unsigned int cmdLen = strlen((char*) userCommand);
+						// if the user simply hit enter then let them try again
+						if((userCommand[0] != '\n') && (userCommand[0] != '\r'))
+						{
+							// remove newline and return line chars
+							if((userCommand[cmdLen - 1] == '\n') || (userCommand[cmdLen - 1] == '\r'))
+							userCommand[cmdLen - 1] = '\0';
+
+							unsigned char *packet;
+							formatCommand(userCommand, &packet);
+
+							printf("received input from cli: '%s'\n", userCommand);
+
+							fprintf(stdout, "CLI sees as: %f\n", getFloat(packet, 7));
+
+							// Write the command to the control_loop socket
+							// int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
+							// if(n < 0) {
+							// 	fprintf(stdout, "CLI: ERROR writing to socket\n");
+							// }
+						}
+						
+
+						fprintf(stdout, "$microcart> ");
+						memset(userCommand, 0, cmdLen);
+					} else if (fd == zyboSocket) {
+						
+					}
+				}
+			}
+		}
+	}
+
+	// ucart_vrpn_tracker_freeInstance(tracker);
+	
+	if (pthread_mutex_lock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+	close(zyboSocket);
+	if (pthread_mutex_unlock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	closeLogFile();
+	return 0;
+}
+
+// signal handler to exit while loop of main function
+void killHandler(int dummy) {
+	keepRunning = 0;
+	printf("\nleaving Bluetooth module\n");
+}
+
+void sendStartPacket() {
+	unsigned char packet[8] = {0};
+
+	metadata_t metadata = 
+	{
+		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] = 1; 			// MSG ID(1)
+	packet[4] =	0; // 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 = sizeof(info) + 8;
+	int n;
+	char packet[pSize];
+	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] = (0x00 & 0x000000ff); 			// MSG ID(1)
+	packet[4] =	((0x00 >> 8) & 0x000000ff); // MSG ID(2)
+	packet[5] = (sizeof(info) & 0x000000ff); 			// DATALEN(1)
+	packet[6] = ((sizeof(info) >> 8) & 0x00000ff); 	// DATALEN(2)
+	memcpy(&packet[7], &info, sizeof(info));
+
+	char checksum = 0;
+	int i;
+	for(i=0; i < pSize - 1; i++)
+		checksum ^= packet[i];
+
+	packet[pSize - 1] = checksum; //PACKET_END_BYTE;
+
+	n = writeQuad(packet, pSize);
+	if(n < 0) {	
+		perror("vrpnhandler: ERROR writing to socket");
+		keepRunning = 0;
+	}
+}
+
+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;
+	struct sockaddr_rc addr = { -1 };
+
+	// 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
+	int status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+
+	// connection failed
+	if(status < 0)
+	{
+		close(sock);
+		printf("Connection failed!...\n");
+		return -1;
+	} 
+	else
+	{
+		printf("connection successful!...\n");
+		return sock;
+	}
+}
+
+void performCommand(char *cmdName, char * command) {
+	for(int i = 0; i < NUM_COMMANDS; ++i)
+	{
+		if(startsWith(registeredCommands[i].commandText, command));
+			fprintf(stdout, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
+	}
+}
+
+int startsWith(const char *pre, const char *str) {
+    size_t lenpre = strlen(pre),
+           lenstr = strlen(str);
+    return lenstr < lenpre ? 0 : (strncmp(pre, str, lenpre) == 0);
+}
+
+/* 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 (pthread_mutex_lock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+	retval = writeQuad(buf, count);
+	if (pthread_mutex_unlock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	return retval;
+}

groundStation/src/old_main.cold

+#include <libcli.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <string.h>
+#include <strings.h>
+#include <signal.h>
+#include <unistd.h>
+#include <arpa/inet.h>
+
+
+#define CLITEST_PORT		8000
+
+int fn_help(struct cli_def *cli, const char *command, char *argv[], int argc);
+
+struct cli_def * cli;
+int on = 1;
+
+int main(int argc, char **argv)
+{
+	int s, x;
+    struct sockaddr_in addr;
+
+	if ((s = socket(AF_INET, SOCK_STREAM, 0)) < 0)
+    {
+        perror("socket");
+        return 1;
+    }
+    setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
+
+    memset(&addr, 0, sizeof(addr));
+    addr.sin_family = AF_INET;
+    addr.sin_addr.s_addr = htonl(INADDR_ANY);
+    addr.sin_port = htons(CLITEST_PORT);
+
+    if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0)
+    {
+        perror("bind");
+        return 1;
+    }
+
+    if (listen(s, 50) < 0)
+    {
+        perror("listen");
+        return 1;
+    }
+
+    printf("Listening on port %d\n", CLITEST_PORT);
+
+	cli = cli_init();
+	cli_set_banner(cli, "Welcome to my test cli");
+
+	cli_register_command(cli, NULL, "helpme", fn_help, PRIVILEGE_UNPRIVILEGED, MODE_EXEC, "displays help info for a command");
+	
+	
+
+
+	while((x = accept(s, NULL, NULL)))
+	{
+		cli_loop(cli, x);
+    close(x);
+	}
+	
+
+	cli_done(cli);
+}
+
+
+int fn_help(struct cli_def *cli, const char *command, char *argv[], int argc) {
+	printf("you ran the help function!!\n");
+    return 0;   
+}
+

groundStation/src/type_def.h

+/*
+ * struct_def.h
+ *
+ *  Created on: Mar 2, 2016
+ *      Author: ucart
+ */
+
+#ifndef TYPE_DEF_H_
+#define TYPE_DEF_H_
+
+/**
+ * @brief
+ *      The modes for autonomous and manual flight.
+ *
+ */
+enum flight_mode{
+	AUTO_FLIGHT_MODE,
+	MANUAL_FLIGHT_MODE
+};
+
+//----------------------------------------------------------------------------------------------
+//     index||     0    |     1    |      2      |  3 & 4 |      5 & 6       |  7+  |   end    |
+//---------------------------------------------------------------------------------------------|
+// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+//-------------------------------------------------------------------------------------------- |
+//     bytes||     1    |     1    |      1      |    2   |        2         | var  |    1     |
+//----------------------------------------------------------------------------------------------
+typedef struct {
+	char begin_char;
+	char msg_type;
+	char msg_subtype;
+	int msg_id;
+	int data_len;
+} metadata_t;
+
+
+// String builder data type
+typedef struct stringBuilder_s {
+	char* buf;
+	int length;
+	int capacity;
+	int maxCapacity;
+
+	// Methods
+	int (*addStr)(struct stringBuilder_s*, char*);
+	int (*addStrAt)(struct stringBuilder_s*, char*, int);
+	int (*addChar)(struct stringBuilder_s*, char);
+	int (*addCharAt)(struct stringBuilder_s*, char, int);
+	int (*removeCharAt)(struct stringBuilder_s*, int);
+	void (*clear)(struct stringBuilder_s*);
+} stringBuilder_t;
+
+typedef struct {
+	char** tokens;
+	int numTokens;
+} tokenList_t;
+
+typedef struct commands{
+	int pitch, roll, yaw, throttle;
+}commands;
+
+typedef struct raw{
+	int x,y,z;
+}raw;
+typedef struct PID_Consts{
+	float P, I, D;
+}PID_Consts;
+
+//Camera system info
+typedef struct {
+	int packetId;
+
+	double y_pos;
+	double x_pos;
+	double alt_pos;
+
+	double yaw;
+	double roll;
+	double pitch;
+} quadPosition_t;
+
+typedef struct {
+	float yaw;
+	float roll;
+	float pitch;
+	float throttle;
+} quadTrims_t;
+
+//Gyro, accelerometer, and magnetometer data structure
+//Used for reading an instance of the sensor data
+typedef struct {
+
+	// GYRO
+	//Xint16 raw_gyro_x, raw_gyro_y, raw_gyro_z;
+
+	float gyro_xVel_p; // In degrees per second
+	float gyro_yVel_q;
+	float gyro_zVel_r;
+
+	// ACCELEROMETER
+	//Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
+
+	float accel_x; //In g
+	float accel_y;
+	float accel_z;
+
+	float accel_roll;
+	float accel_pitch;
+
+
+	// MAG
+	//Xint16 raw_mag_x, raw_mag_y, raw_mag_z;
+
+	float heading; // In degrees
+
+	float mag_x; //Magnetic north: ~50 uT
+	float mag_y;
+	float mag_z;
+
+
+
+}gam_t;
+
+typedef struct PID_t {
+	double current_point;	// Current value of the system
+	double setpoint;		// Desired value of the system
+	float Kp;				// Proportional constant
+	float Ki;				// Integral constant
+	float Kd;				// Derivative constant
+	double prev_error;		// Previous error
+	double acc_error;		// Accumulated error
+	double pid_correction;	// Correction factor computed by the PID
+	float dt; 				// sample period
+} PID_t;
+
+typedef struct PID_values{
+	float P;	// The P component contribution to the correction output
+	float I;	// The I component contribution to the correction output
+	float D;	// The D component contribution to the correction output
+	float error; // the error of this PID calculation
+	float change_in_error; // error change from the previous calc. to this one
+	float pid_correction; // the correction output (P + I + D)
+} PID_values;
+
+///////// MAIN MODULAR STRUCTS
+/**
+ * @brief
+ *      Holds the data inputed by the user
+ *
+ */
+typedef struct user_input_t {
+	int rc_commands[6]; 	// Commands from the RC transmitter
+
+
+//	float cam_x_pos;	// Current x position from the camera system
+//	float cam_y_pos;	// Current y position from the camera system
+//	float cam_z_pos;	// Current z position from the camera system
+//	float cam_roll;		// Current roll angle from the camera system
+//	float cam_pitch;	// Current pitch angle from the camera system
+//	float cam_yaw;		// Current yaw angle from the camera system
+
+	float yaw_manual_setpoint;
+	float roll_angle_manual_setpoint;
+	float pitch_angle_manual_setpoint;
+
+	int hasPacket;
+	stringBuilder_t * sb;
+} user_input_t;
+
+/**
+ * @brief
+ *      Holds the log data to be sent to the ground station. It may hold the
+ * timestamp of when a sensor's data was obtained.
+ *
+ */
+typedef struct log_t {
+	// Time
+	float time_stamp;
+	float time_slice;
+
+	// Id
+	int packetId;
+
+	gam_t gam; 	// Raw and calculated gyro, accel, and mag values are all in gam_t
+	float phi_dot, theta_dot, psi_dot; // gimbal equation values
+
+	quadPosition_t currentQuadPosition;
+
+	float roll_angle_filtered, pitch_angle_filtered;
+	float lidar_altitude;
+
+	float pid_P_component, pid_I_component, pid_D_component; // use these generically for any PID that you are testing
+
+	// PID coefficients and errors
+	PID_t local_x_PID, local_y_PID, altitude_PID;
+	PID_t angle_yaw_PID, angle_roll_PID, angle_pitch_PID;
+	PID_t ang_vel_yaw_PID, ang_vel_roll_PID, ang_vel_pitch_PID;
+
+	PID_values local_x_PID_values, local_y_PID_values, altitude_PID_values;
+	PID_values angle_yaw_PID_values, angle_roll_PID_values, angle_pitch_PID_values;
+	PID_values ang_vel_yaw_PID_values, ang_vel_roll_PID_values, ang_vel_pitch_PID_values;
+
+	// RC commands
+	commands commands;
+
+	//trimmed values
+	quadTrims_t trims;
+
+	int motors[4];
+
+} log_t;
+
+/**
+ * @brief
+ *      Holds the raw data from the sensors and the timestamp if available
+ *
+ */
+typedef struct raw_sensor {
+	int acc_x;		// accelerometer x data
+	int acc_x_t;	// time of accelerometer x data
+
+	int acc_y;		// accelerometer y data
+	int acc_y_t;	// time of accelerometer y data
+
+	int acc_z;		// accelerometer z data
+	int acc_z_t;	// time of accelerometer z data
+
+
+	int gyr_x;		// gyroscope x data
+	int gyr_x_t;	// time of gyroscope x data
+
+	int gyr_y;		// gyroscope y data
+	int gyr_y_t;	// time of gyroscope y data
+
+	int gyr_z;		// gyroscope z data
+	int gyr_z_t;	// time of gyroscope z data
+
+	int ldr_z;		//lidar z data (altitude)
+	int ldr_z_t;	//time of lidar z data
+
+	gam_t gam;
+
+	// Structures to hold the current quad position & orientation
+	quadPosition_t currentQuadPosition;
+
+} raw_sensor_t;
+
+/**
+ * @brief
+ *      Holds the processed data from the sensors and the timestamp if available
+ *
+ */
+typedef struct sensor {
+	int acc_x;		// accelerometer x data
+	int acc_x_t;	// time of accelerometer x data
+
+	int acc_y;		// accelerometer y data
+	int acc_y_t;	// time of accelerometer y data
+
+	int acc_z;		// accelerometer z data
+	int acc_z_t;	// time of accelerometer z data
+
+
+	int gyr_x;		// gyroscope x data
+	int gyr_x_t;	// time of gyroscope x data
+
+	int gyr_y;		// gyroscope y data
+	int gyr_y_t;	// time of gyroscope y data
+
+	int gyr_z;		// gyroscope z data
+	int gyr_z_t;	// time of gyroscope z data
+
+	int ldr_z;		//lidar z data (altitude)
+	int ldr_z_t;	//time of lidar z data
+
+	float pitch_angle_filtered;
+	float roll_angle_filtered;
+	float lidar_altitude;
+
+	float phi_dot, theta_dot, psi_dot;
+
+	// Structures to hold the current quad position & orientation
+	quadPosition_t currentQuadPosition;
+	quadTrims_t trims;
+
+} sensor_t;
+
+/**
+ * @brief
+ *      Holds the setpoints to be used in the controller
+ *
+ */
+typedef struct setpoint_t {
+	quadPosition_t desiredQuadPosition;
+} setpoint_t;
+
+/**
+ * @brief
+ *      Holds the parameters that are specific to whatever type of
+ *      control algorithm is being used
+ *
+ */
+typedef struct parameter_t {
+	PID_t roll_angle_pid, roll_ang_vel_pid;
+	PID_t pitch_angle_pid, pitch_ang_vel_pid;
+	PID_t yaw_ang_vel_pid;
+	PID_t local_x_pid;
+	PID_t local_y_pid;
+	PID_t yaw_angle_pid;
+	PID_t alt_pid;
+} parameter_t;
+
+/**
+ * @brief
+ *      Holds user defined data for the controller
+ *
+ */
+typedef struct user_defined_t {
+	int flight_mode;
+	int engaging_auto;
+} user_defined_t;
+
+/**
+ * @brief
+ *      Holds the raw actuator values before processing
+ *
+ */
+typedef struct raw_actuator_t {
+
+	int controller_corrected_motor_commands[6];
+
+} raw_actuator_t;
+
+/**
+ * @brief
+ *      Holds processed commands to go to the actuators
+ *
+ */
+typedef struct actuator_command_t {
+	int pwms[4];
+} actuator_command_t;
+
+/**
+ * @brief
+ * 		Structures to be used throughout
+ */
+typedef struct {
+	user_input_t user_input_struct;
+	log_t log_struct;
+	raw_sensor_t raw_sensor_struct;
+	sensor_t sensor_struct;
+	setpoint_t setpoint_struct;
+	parameter_t parameter_struct;
+	user_defined_t user_defined_struct;
+	raw_actuator_t raw_actuator_struct;
+	actuator_command_t actuator_command_struct;
+}modular_structs_t;
+
+//////// END MAIN MODULAR STRUCTS
+
+#endif /* TYPE_DEF_H_ */

groundStation/src/vrpn_tracker.cpp

+#include <iostream>
+#include <algorithm>
+
+#include "vrpn_Tracker.h"
+#include "quat.h"
+
+#include "vrpn_tracker.hpp"
+
+namespace microcart
+{
+	static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);
+
+	Tracker::Tracker(std::string server) : Tracker(server.c_str())
+	{
+	}
+
+	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
+			}
+		})
+
+	{
+		remote.register_change_handler(this, vrpn_cb);
+
+		stop_flag = 0;
+		vrpn_thread = std::thread(&Tracker::vrpn_loop, this);
+	}
+
+	Tracker::~Tracker()
+	{
+		{
+			std::lock_guard<std::mutex> guard(vrpn_mutex);
+			stop_flag = 1;
+		}
+		vrpn_thread.join();
+	}
+
+	const struct TrackerData Tracker::getData(void)
+	{
+		std::lock_guard<std::mutex> guard(vrpn_mutex);
+		return trackerData;
+	}
+
+	void Tracker::vrpn_loop(void)
+	{
+		while (1) {
+			remote.mainloop();
+			{
+				std::lock_guard<std::mutex> guard(vrpn_mutex);
+
+				if (stop_flag) {
+					return;
+				}
+			}
+		}
+	}
+
+	void Tracker::callback(const vrpn_TRACKERCB t)
+	{
+		std::lock_guard<std::mutex> guard(vrpn_mutex);
+
+		q_vec_type euler;
+		q_to_euler(euler, t.quat);
+
+		trackerData.x = (double) t.pos[0];
+		trackerData.y = (double) t.pos[1];
+		trackerData.z = (double) t.pos[2];
+
+		trackerData.roll  = (double) euler[2];
+		trackerData.pitch = (double) euler[1];
+		trackerData.yaw   = (double) euler[0];
+
+		timeval elapsed_time;
+		timersub(&t.msg_time, &trackerData.timestamp, &elapsed_time);
+		trackerData.timestamp = t.msg_time;
+
+		double elapsed_time_usec = (double) elapsed_time.tv_usec + 
+			((1000000.0) * (double) elapsed_time.tv_sec);
+
+		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);
+		});
+	}
+
+	void Tracker::addCallback(std::function<void(const TrackerData&)> cb) 
+	{
+		cb_vector.push_back(cb);
+	}
+
+	static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t)
+	{
+		Tracker * inst = (Tracker *)(param);
+		inst->callback(t);
+	}
+}
+
+/* C Interface stuff */
+struct ucart_vrpn_tracker {
+	microcart::Tracker * t;
+};
+
+extern "C"
+{
+	struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
+			const char * server)
+	{
+		try {
+			auto inst = new struct ucart_vrpn_tracker;
+			inst->t = new microcart::Tracker(server);
+		} catch(...) {
+			return NULL;
+		}
+	}
+
+	void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst)
+	{
+		delete inst->t;
+		delete inst;
+	}
+
+	int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
+			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);
+					});
+		} catch(...) {
+			return -1;
+		}
+		return 0;
+	}
+
+	int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker *inst,
+			struct ucart_vrpn_TrackerData *td)
+	{
+		try {
+			auto data = inst->t->getData();
+			td->x = data.x;
+			td->y = data.y;
+			td->z = data.z;
+			td->pitch = data.pitch;
+			td->roll = data.roll;
+			td->yaw = data.yaw;
+
+			return 0;
+		} catch(...) {
+			return -1;
+		}
+	}
+}

groundStation/src/vrpn_tracker.hpp

+/* Author: Jake Drahos
+ *
+ * VPRN tracker module header file.
+ */
+
+#ifndef _MICROCART_VRPN_TRACKER_HPP
+#define _MICROCART_VRPN_TRACKER_HPP
+
+#ifdef __cplusplus
+#include <mutex>
+#include <thread>
+#include <functional>
+#include <vector>
+
+#include "vrpn_Tracker.h"
+#endif
+
+#include <sys/time.h>
+
+#ifdef __cplusplus
+extern "C" 
+{
+#endif
+	struct ucart_vrpn_tracker;
+	struct ucart_vrpn_TrackerData {
+		double x;
+		double y;
+		double z;
+
+		double pitch;
+		double roll;
+		double yaw;
+
+		double fps;
+		struct timeval timestamp;
+	};
+
+	struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
+			const char * server);
+
+	void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst);
+
+	int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
+			void (*cb)(struct ucart_vrpn_TrackerData *));
+
+	int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker * inst,
+			struct ucart_vrpn_TrackerData * td);
+
+#ifdef __cplusplus
+}
+#endif
+
+#ifdef __cplusplus
+namespace microcart
+{
+	struct TrackerData {
+		double x;
+		double y;
+		double z;
+
+		double pitch;
+		double roll;
+		double yaw;
+
+		double fps;
+		timeval timestamp;
+	};
+
+	class Tracker {
+	public:
+		const struct TrackerData getData(void);
+		void callback(const vrpn_TRACKERCB t);
+
+		Tracker(std::string server);
+		Tracker(const char * server);
+		~Tracker();
+
+		void addCallback(std::function<void(const TrackerData &)> cb);
+	private:
+		int stop_flag;
+		TrackerData trackerData;
+		std::thread vrpn_thread;
+		std::mutex vrpn_mutex;
+
+		vrpn_Tracker_Remote remote;
+
+		void vrpn_loop(void);
+
+		std::vector<std::function<void(const TrackerData &)>> cb_vector;
+	};
+
+
+}
+/* __cplusplus */
+#endif
+
+#endif