.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

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 vrpn/build $(EXE)
-
-# 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)

README.md

-# groundStation
\ No newline at end of file

Readme.md

+This is a readme, please update accordingly.
\ No newline at end of file

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

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

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;
-}

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

src/logger.c

-/* Author: Kris Burney
- *
- * Logger file for holding functions pertaining to loging to a file.
- */
-#include "logger.h"
-#include <stdio.h>
-
-int quadlog_file;
-
-int createLogFile(int argc, char* argv)
-{
-	char log_file[300] = {'l', 'o', 'g','s', '/'};
-	if(argc >= 2)
-	{
-		strcat(log_file, argv);
-		printf("Creating log file '%s'...\n",log_file);
-		quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666);
-		return quadlog_file;
-	}
-	else
-	{
-		time_t rawtime;
-		char timestr [30];
-		time(&rawtime);
-		sprintf(timestr,"%s",ctime(&rawtime));
-
-		// Lets convert space to _ in
-		char *p = timestr;
-		int 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 = open(log_file, O_WRONLY | O_CREAT, 0666);
-		return quadlog_file;
-	}
-}
-
-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",
-		td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
-}
-
-int writeStringToLog(const char * string)
-{
-	return dprintf(quadlog_file, "%s", string);
-}

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* );
-
-
-#endif
\ No newline at end of file

src/microcart_cli.c

-/* Author: Kris Burney
- *
- * BlueTooth socket program for passing vrpn data to quad.
- */
-//system includes
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <signal.h>
-#include <sys/socket.h>
-#include <bluetooth/bluetooth.h>
-#include <bluetooth/rfcomm.h>
-#include <pthread.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 *cmdName, char * command);
-int startsWith(const char *pre, const char *str);
-
-//static void cb(struct ucart_vrpn_TrackerData *);
-
-// global variables
-static volatile int keepRunning = 1;
-const char *TRACKER_IP = "UAV@192.168.0.120:3883";
-int quadlog_file;
-int zyboSocket, status, bytes_read;
-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);
-// 	}**/
-// }
-
-
-void *handleQuadResponse() {
-	unsigned char buffer[255];
-
-	while(keepRunning) {
-		// Clear the buffer and read the message from the socket (from the server)
-		memset(buffer, 0, 255);
-
-		// If there was an error reading from the socket, throw an error
-		if(read(zyboSocket, buffer, 255) <= 0) {
-			fprintf(stderr, "CLI QUAD: ERROR reading from quad.\n");
-			continue;
-		}
-
-		pthread_mutex_lock(&quadResponseMutex);
-		newQuadResponse = 1;
-		memcpy(respBuf, buffer, 255);
-		pthread_mutex_unlock(&quadResponseMutex);
-
-		//parse_packet(buffer, &respBuf, &metadata);
-		// fprintf(stderr, "CLI QUAD: %s\n", buffer);
-	}
-
-	pthread_exit(NULL);
-}
-
-void *handleCliInput() {
-	sleep(1);
-	while(keepRunning)
-	{
-		char userCommand[CMD_MAX_LENGTH] = {};
-		fprintf(stderr, "$microcart> ");
-
-		while(fgets(userCommand, sizeof(userCommand), stdin) != NULL && keepRunning) {
-			
-			// if the user simply hit enter then let them try again
-			if((userCommand[0] == '\n') || (userCommand[0] == '\r'))
-			{
-				fprintf(stderr, "$microcart> ");
-
-				memset(userCommand, 0, CMD_MAX_LENGTH);
-
-				continue;
-			}
-
-			if((userCommand[strlen(userCommand) - 1] == '\n') || (userCommand[strlen(userCommand) - 1] == '\r'))
-			userCommand[strlen(userCommand) - 1] = '\0';
-
-			pthread_mutex_lock(&cliInputMutex);
-			newCliInput = 1;
-			memcpy(commandBuf, &userCommand, CMD_MAX_LENGTH);
-			pthread_mutex_unlock(&cliInputMutex);
-
-			fprintf(stderr, "$microcart> ");
-
-			memset(userCommand, 0, CMD_MAX_LENGTH);
-		}
-	}
-
-	pthread_exit(NULL);
-}
-
-int main(int argc, char **argv)
-{
-	pthread_t quadResponse, cliInput;
-	respBuf = malloc(1024);
-	commandBuf = malloc(CMD_MAX_LENGTH);
-	
-	signal(SIGINT, killHandler);
-
-	if ((zyboSocket = connectToZybo()) < 0)
-	{
-		perror("Error connecting to Zybo...");
-		free(respBuf);
-		free(commandBuf);
-		exit(1);
-	}
-	
-	// create vrpnTracker instance
-//	tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
-
-	// Retrieve VRPN data from the control loop
-	fprintf(stderr, "CLI: Starting quad receiving thread...\n");
-	pthread_create(&quadResponse, NULL, handleQuadResponse, NULL);
-	fprintf(stderr, "CLI: Thread quad receiving started.\n");
-
-	// Retrieve user command input
-	fprintf(stderr, "CLI: Starting cli input thread...\n");
-	pthread_create(&cliInput, NULL, handleCliInput, NULL);
-	fprintf(stderr, "CLI: Thread Cli input started.\n");
-
-	// open the log file
-	if( (status = createLogFile(argc, argv[1])) < 0)
-	{
-		perror("Error creating log file...");
-		free(respBuf);
-		free(commandBuf);
-		exit(1);
-	}
-	writeStringToLog(logHeader);
-
-	//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);
-	int updatePrompt = 0;
-	while(keepRunning)
-	{
-		char tmpRespBuf[1024];
-		unsigned char tmpCommandBuf[CMD_MAX_LENGTH];
-
-
-		if(updatePrompt)
-		{
-			fprintf(stderr, "$microcart> ");
-			updatePrompt = 0;	
-		}
-
-		//check for user input via cli
-		if(atomic_check(&newCliInput, &cliInputMutex))
-		{
-			pthread_mutex_lock(&cliInputMutex);
-			newCliInput = !newCliInput;
-			memcpy(tmpCommandBuf, commandBuf, CMD_MAX_LENGTH);
-			pthread_mutex_unlock(&cliInputMutex);
-
-			// I can use printf becuase the command was gathered using fgets.
-			//fprintf(stderr, "\rINPUT FOUND via CLI: '%s'\n", tmpCommandBuf);
-			updatePrompt = !updatePrompt;
-			unsigned char *packet;
-			formatCommand(tmpCommandBuf, &packet);
-		}
-
-		//check for update/response from quad
-		if(atomic_check(&newQuadResponse, &quadResponseMutex))
-		{
-			pthread_mutex_lock(&quadResponseMutex);
-			newQuadResponse = !newQuadResponse;
-			memcpy(tmpRespBuf, respBuf, 1024);
-			pthread_mutex_unlock(&quadResponseMutex);
-
-			char buf[1025];
-			memcpy(buf, tmpRespBuf, 1024);
-			buf[1025] = '\0';
-			//fprintf(stderr, "\rINPUT FOUND via QUAD: '%s'\n", buf);
-			writeStringToLog(buf);
-			updatePrompt = !updatePrompt;
-		}
-	}
-
-	//ucart_vrpn_tracker_freeInstance(tracker);
-	//free(vrpnData);
-	free(respBuf);
-	free(commandBuf);
-	pthread_mutex_destroy(&cliInputMutex);
-	pthread_mutex_destroy(&quadResponseMutex);
-	close(zyboSocket);
-	close(quadlog_file);
-	return 0;
-}
-
-// signal handler to exit while loop of main function
-void killHandler(int dummy) {
-	keepRunning = 0;
-	printf("\nleaving Bluetooth module\n");
-}
-
-void readAndPrint() {
-	// read data from the server
-	// this is a blocking call.
-	//TODO: Implement a non blocking version of this.
-	bytes_read = read(zyboSocket, respBuf, sizeof(respBuf) -1);
-	if( bytes_read > 0)
-	{
-		respBuf[bytes_read] = '\0';
-		printf("%s", respBuf);
-	}
-}
-
-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;
-	status = write(zyboSocket, &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 = write(zyboSocket, 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
-	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;
-	}
-}
-
-int atomic_check(int* atomicFlag, pthread_mutex_t* mutex) {
-   pthread_mutex_lock(mutex);
-   int result = *atomicFlag;
-   pthread_mutex_unlock(mutex);
-   return result;
-}
-
-void performCommand(char *cmdName, char * command) {
-	for(int i = 0; i < NUM_COMMANDS; ++i)
-	{
-		if(startsWith(registeredCommands[i].commandText, command));
-			fprintf(stderr, "\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);
-}
\ No newline at end of file

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;   
-}
-

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_ */

src/vrpn/.clang-format

----
-Language:        Cpp
-BasedOnStyle:  LLVM
-Standard:        Auto
-IndentWidth:     4
-TabWidth:        4
-UseTab:          Never
-AccessModifierOffset: -4
-AllowShortIfStatementsOnASingleLine: true
-BreakBeforeBraces: Stroustrup
-BreakConstructorInitializersBeforeComma: true
-NamespaceIndentation: All
-DerivePointerBinding: true
-...
-

src/vrpn/.clang-tidy

----
-# Remove these blocked checks once the first batch are cleaned up
-# - readability-braces-around-statements
-Checks:          '*,-clang-analyzer-alpha*,-llvm-include-order,-google-*,-llvm-header-guard,-readability-braces-around-statements,-misc-use-override'
-HeaderFilterRegex: '.*'
-...
-

src/vrpn/.gitmodules

-[submodule "submodules/hidapi"]
-	path = submodules/hidapi
-	url = https://github.com/vrpn/hidapi.git
-[submodule "submodules/jsoncpp"]
-	path = submodules/jsoncpp
-	url = https://github.com/vrpn/jsoncpp.git

src/vrpn/.travis.yml

-compiler:
-  - clang
-  - gcc
-
-before_install:
-  - git submodule update --init --recursive
-  - sudo apt-get update -qq
-  - sudo apt-get install -qq libgpm-dev freeglut3-dev libxmu-dev libxi-dev libusb-1.0-0-dev libqt4-dev
-
-language: cpp
-script: mkdir build && cd build && cmake -DVRPN_GPL_SERVER=TRUE -D  VRPN_BUILD_EXTRA_COMPILER_WARNINGS=TRUE .. && make && make test