diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..367bf3b3faf0304e135baa9082caab2f6c0ea3cd
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,42 @@
+# 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
diff --git a/groundStation/Makefile b/groundStation/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..f09d3d4d90866ebe6bad0d63c825073e11c6bd66
--- /dev/null
+++ b/groundStation/Makefile
@@ -0,0 +1,55 @@
+# 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)
diff --git a/groundStation/README.md b/groundStation/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..f89fed232d356c457ad4729994eecfe3ee4cbd41
--- /dev/null
+++ b/groundStation/README.md
@@ -0,0 +1 @@
+# groundStation
\ No newline at end of file
diff --git a/groundStation/src/commands.c b/groundStation/src/commands.c
new file mode 100644
index 0000000000000000000000000000000000000000..dc4749ee334f38da4383ca7d8200bbafa429a2b4
--- /dev/null
+++ b/groundStation/src/commands.c
@@ -0,0 +1,109 @@
+#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
diff --git a/groundStation/src/commands.h b/groundStation/src/commands.h
new file mode 100644
index 0000000000000000000000000000000000000000..050990de386d1132419c230744316033805f33af
--- /dev/null
+++ b/groundStation/src/commands.h
@@ -0,0 +1,69 @@
+#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
diff --git a/groundStation/src/communication.c b/groundStation/src/communication.c
new file mode 100644
index 0000000000000000000000000000000000000000..305a40598d8304f6da94e556c4391eda951ceb19
--- /dev/null
+++ b/groundStation/src/communication.c
@@ -0,0 +1,268 @@
+#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;
+}
diff --git a/groundStation/src/communication.h b/groundStation/src/communication.h
new file mode 100644
index 0000000000000000000000000000000000000000..94a3c33c5131ebf2a8c1c4d506bb8e7e5cf62655
--- /dev/null
+++ b/groundStation/src/communication.h
@@ -0,0 +1,21 @@
+#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
diff --git a/groundStation/src/logger.c b/groundStation/src/logger.c
new file mode 100644
index 0000000000000000000000000000000000000000..028cfeac16c173e59154a1c9c8321eeba29d0344
--- /dev/null
+++ b/groundStation/src/logger.c
@@ -0,0 +1,108 @@
+/* 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__);
+ }
+
+}
diff --git a/groundStation/src/logger.h b/groundStation/src/logger.h
new file mode 100644
index 0000000000000000000000000000000000000000..edfb365ffce6606e7b5bd8d1f4305c5f697180a9
--- /dev/null
+++ b/groundStation/src/logger.h
@@ -0,0 +1,21 @@
+/* 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
diff --git a/groundStation/src/microcart_cli.c b/groundStation/src/microcart_cli.c
new file mode 100644
index 0000000000000000000000000000000000000000..bf2fbc9567ebbf6507a7280e133ec2779b438bd6
--- /dev/null
+++ b/groundStation/src/microcart_cli.c
@@ -0,0 +1,365 @@
+/* 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;
+}
diff --git a/groundStation/src/old_main.cold b/groundStation/src/old_main.cold
new file mode 100644
index 0000000000000000000000000000000000000000..5234bcc567a6e574b30eea671b3f977a7211f478
--- /dev/null
+++ b/groundStation/src/old_main.cold
@@ -0,0 +1,73 @@
+#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;
+}
+
diff --git a/groundStation/src/type_def.h b/groundStation/src/type_def.h
new file mode 100644
index 0000000000000000000000000000000000000000..55dca9e8fb27c26397b00961b7dbec8bbe0d4136
--- /dev/null
+++ b/groundStation/src/type_def.h
@@ -0,0 +1,361 @@
+/*
+ * 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_ */
diff --git a/groundStation/src/vrpn_tracker.cpp b/groundStation/src/vrpn_tracker.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..49cbaed12e4599a22cecf95ad6351d51de9c6bf5
--- /dev/null
+++ b/groundStation/src/vrpn_tracker.cpp
@@ -0,0 +1,174 @@
+#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;
+ }
+ }
+}
diff --git a/groundStation/src/vrpn_tracker.hpp b/groundStation/src/vrpn_tracker.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ebd217551dab4fa6d26faf8ab1bd14a193aac317
--- /dev/null
+++ b/groundStation/src/vrpn_tracker.hpp
@@ -0,0 +1,97 @@
+/* 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