quad: get most recent groundStation code

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
+logs
+BackEnd
+obj
+Cli

groundStation/Makefile

+# Declaration of variables
+
+# Generic Variables
+GCC=gcc
+GXX=g++
+CFLAGS= -Wall -pedantic -Wextra -Werror -std=gnu99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
+CXXFLAGS= -Wall -pedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
+INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
+INCDIR=inc src/vrpn src/vrpn/quat src/vrpn/build $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR)
+LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat 
+OBJDIR=obj
+
+# Backend Specific Variables
+BEBINARY=BackEnd
+BESRCDIR=src/backend
+BECSOURCES := $(wildcard $(BESRCDIR)/*.c )
+BECOBJECTS = $(BECSOURCES:$(BESRCDIR)/%.c=$(OBJDIR)/%.o)
+BECPPSOURCES := $(wildcard $(BESRCDIR)/*.cpp )
+BECPPOBJECTS = $(BECPPSOURCES:$(BESRCDIR)/%.cpp=$(OBJDIR)/%.o)
+
+# CLI Specific Variables
+CLIBINARY=Cli
+CLISRCDIR=src/cli
+CLISOURCES := $(wildcard $(CLISRCDIR)/*.c)
+CLIOBJECTS = $(CLISOURCES:$(CLISRCDIR)/%.c=$(OBJDIR)/%.o)
+
+# Frontend-common stuff
+FESRCDIR=src/frontend
+FECSOURCES := $(wildcard $(FESRCDIR)/*.c )
+FECOBJECTS = $(FECSOURCES:$(FESRCDIR)/%.c=$(OBJDIR)/%.o)
+
+
+OBJECTS= $(CLIOBJECTS) $(BECOBJECTS) $(BECPPOBJECTS) $(FECOBJECTS)
+
+# Default target
+all: logs objdir backend cli
+
+vrpn: vrpn/build
+
+cli:  $(CLIOBJECTS) $(FECOBJECTS)
+	$(GCC) $(CFLAGS) $^ -o $(CLIBINARY) $(INCLUDES) $(LIBS)
+
+$(CLIOBJECTS) : $(OBJDIR)/%.o : $(CLISRCDIR)/%.c
+	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
+
+backend: $(BECPPOBJECTS) $(BECOBJECTS)
+	$(GXX) $(CXXFLAGS) $^ -o $(BEBINARY) $(INCLUDES) $(LIBS)
+
+$(FECOBJECTS) : $(OBJDIR)/%.o : $(FESRCDIR)/%.c
+	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
+$(BECOBJECTS) : $(OBJDIR)/%.o : $(BESRCDIR)/%.c
+	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
+$(BECPPOBJECTS) : $(OBJDIR)/%.o : $(BESRCDIR)/%.cpp
+	$(GXX)  $(CXXFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
+vrpn/build:
+	mkdir -p src/vrpn/build
+	cd src/vrpn/build && cmake .. && make
+
+logs:
+	mkdir -p logs
+
+objdir:
+	mkdir -p obj
+
+clean_logs:
+	rm -f logs/*
+
+clean:
+	rm -rf $(OBJDIR)/ $(BEBINARY) $(CLIBINARY)
+
+debug:
+	@echo $(OBJECTS)

groundStation/README.md

+# groundStation
+
+## Make Process
+
+First, if submodules were not recursevly added through git. Run this command if you have made any attempt to make vrpn manually. 
+run
+	'git submodule update --init --recursive'
+
+Now that you have all of the files necissary.
+
+cd into the groundstation folder.
+	cd groundStation
+	make vrpn
+	make
+
+run the program with sudo privledges
+	sudo -E ./BackEnd
+
+If you wish to change the way the backend communicates to the quad and vice versa, look at src/config.h. 
+	This provides a list of environment variables which you can set and use for your computer or time of use.
+	Because the backend must be ran with sudo privledges, you will need to preserve the env. vars. with sudo rights.
+	Hence the "-E" flag.
+
+## Modifying
+See MODIFYING for the software architecture/organization and how to add new functionality.
+
+## Using
+First, the backend daemon must be running. Run the backend with ./BackEnd. Note
+the environment variables in config.h, especially the backend socket path. The backend
+requires root for bluetooth, but can run as a normal user with TCP, as long as the
+socket path is writable by the user.
+
+Once the backend is running, various CLI tools can be used to view the state of the
+quad and send commands. Each of these tools is given as the first argument
+to the CLI program, for example, to monitor the quad, use `cli monitor`. Note that
+the backend socket environment variable must be set to the same value as it
+was for the backend. The CLI program also supports busybox-style symbolic links.
+For example, if there is a symlink named `monitor` that points to the `cli` binary,
+running the `monitor` program will effectively run `cli monitor`.
+
+The names of the binaries is subject to change.
+
+### Example
+In one terminal or screen, run the backend:
+
+`UCART_SOCKET=./ucart.socket ./BackEnd`
+
+This will activate the quad and the backend, and the backend will be available for
+connections from the frontend tools. One useful tool is the monitor. In another
+terminal window, run the monitor forever:
+
+`UCART_SOCKET=./ucart.socket ./cli monitor -f`
+
+This will begin a periodic monitoring that updates 10 times per second.
+
+Finally, in a third window, export the socket path:
+
+`export UCART_SOCKET=./ucart.socket`
+
+and then run any other tools to modify the quad, for example modifying PID constants:
+
+`./cli setpid --pitch -p 1.000`
+

groundStation/src/backend/commands.h

+#ifndef _COMMANDS_H
+#define _COMMANDS_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "type_def.h"
+
+
+// ----------------------
+// Helper stuff
+
+#define MAX_TYPE 6
+#define MAX_SUBTYPE 100
+
+enum Message{
+	BEGIN_CHAR = 0xBE,
+	END_CHAR   = 0xED
+};
+
+// This should also have double to avoid confusion with float values.
+enum DataType
+{
+	floatType,
+	intType,
+	stringType
+};
+
+// MESSAGE SUBTYPES
+struct MessageSubtype{
+	char ID;
+	char cmdText[100];
+	char cmdDataType;
+	int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
+};
+
+// MESSAGE TYPES
+struct MessageType{
+	char ID;
+	struct MessageSubtype subtypes[MAX_SUBTYPE];
+};
+
+int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
+int update(unsigned char *c, int dataLen, modular_structs_t *structs);
+int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
+int response(unsigned char *packet, int dataLen, modular_structs_t *structs);
+int setyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setroll(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
+int setthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getroll(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getpitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int getrollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resproll(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resprollp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resprolld(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resppitch(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resppitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resppitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
+int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
+
+float getFloat(unsigned char* str, int pos);
+int getInt(unsigned char* str, int pos);
+
+// TODO add in string to be read from the command line when sending a subtype of message
+extern struct MessageType MessageTypes[MAX_TYPE];
+
+#endif
\ No newline at end of file

groundStation/src/backend/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(char *command, unsigned char **formattedCommand) {
+	//fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
+	char cmd[strlen(command)];
+	strncpy(cmd, command, strlen(command));
+	cmd[strlen(command)] = '\0';
+
+	tokenList_t tokens = tokenize(cmd);
+	float floatValue = 0.0;
+	int intValue = 0;
+	int valid;
+	metadata_t metadata;
+	
+	// ----------------------------------------------
+	if(tokens.numTokens > 0) {
+		for(int type = 0; type < MAX_TYPE; type++)
+		{
+			for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
+			{
+				if(strcmp(tokens.tokens[0], MessageTypes[type].subtypes[subtype].cmdText) == 0)
+				{
+					printf("Sending\n\ttype: %d, \n\tsubtype: %d\n\tcommand: %s\n", type, subtype, MessageTypes[type].subtypes[subtype].cmdText);
+					
+					// Make sure the second token is the right type
+					switch (MessageTypes[type].subtypes[subtype].cmdDataType)
+					{
+						// Validate the float input
+						case floatType:
+							metadata.begin_char = (char) BEGIN_CHAR;
+							metadata.msg_type = MessageTypes[type].ID;
+							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+
+							if(MessageTypes[type].ID == 0x01) {
+								valid = checkFloat(tokens.tokens[1], &floatValue);
+								if(!valid) {
+									return -1;
+								}
+								metadata.data_len = sizeof(floatValue);
+							} else {
+								metadata.data_len = 0;
+							}
+							
+							metadata.msg_id = msgNum++;
+							formatPacket(&metadata, &floatValue, formattedCommand);
+							break;
+						
+						// Validate the integer input
+						case intType:
+							metadata.begin_char = (char) BEGIN_CHAR;
+							metadata.msg_type = MessageTypes[type].ID;
+							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+							
+							if(MessageTypes[type].ID == 0x01) {
+								valid = checkInt(tokens.tokens[1], &intValue);
+								if(!valid) {
+									return -1;
+								}
+								metadata.data_len = sizeof(intValue);
+							} else {
+								metadata.data_len = 0;
+							}
+							
+							metadata.msg_id = msgNum++;
+							formatPacket(&metadata, &intValue, formattedCommand);
+							
+							break;
+						
+						// Validate the string input (doesn't need to happen)
+						case stringType:
+							metadata.begin_char = (char) BEGIN_CHAR;
+							metadata.msg_type = MessageTypes[type].ID;
+							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+							metadata.msg_id = msgNum++;
+							metadata.data_len = strlen(tokens.tokens[1]);
+							
+							formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
+							
+							break;
+						default:
+							return -1;
+					}
+					return 0;
+				}
+			}
+		}
+	}
+
+	// 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 & 0x000000ff);
+	(*formattedCommand)[4] = ((metadata->msg_id >> 8) & 0x000000ff);
+	
+	// Data length and data - bytes 5&6 for len, 7+ for data
+	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
+	(*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, modular_structs_t *structs) {
+	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) {
+		/* Null check*/
+		if (MessageTypes[(unsigned char)metadata.msg_type].subtypes[
+				(unsigned char) metadata.msg_subtype].functionPtr) {
+			// Call the appropriate subtype function
+			(* (MessageTypes[(unsigned char)metadata.msg_type].subtypes[(unsigned char)metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
+		}
+			
+		return 0;
+	}
+	
+	// Only gets here if there is an error
+	return -1;
+}

groundStation/src/backend/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(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, modular_structs_t *structs);
+int logData(unsigned char *log_msg, unsigned char *formattedCommand);
+
+#endif
\ No newline at end of file

groundStation/src/backend/config.h

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

groundStation/src/backend/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/backend/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/backend/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/backend/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/backend/vrpn_tracker.cpp

+#include <iostream>
+#include <algorithm>
+#include <functional>
+
+#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);
+	
+	TrackerData::TrackerData() :
+		x(0.0), y(0.0), z(0.0),
+		pitch(0.0), roll(0.0), yaw(0.0),
+		fps(0.0), timestamp()
+	{
+	}
+
+	Tracker::Tracker(std::string server) : 
+		remote(server.c_str()),
+		stop_flag(0),
+		trackerData()
+	{
+	}
+
+	Tracker::Tracker(const char * server) :
+		remote(server),
+		stop_flag(0),
+		trackerData()
+	{
+		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;
+		for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
+			(*i)(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;
+};
+
+void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
+		const microcart::TrackerData &td)
+{
+						struct ucart_vrpn_TrackerData data;
+						data.x = td.x;
+						data.y = td.y;
+						data.z = td.z;
+						data.pitch = td.pitch;
+						data.roll = td.roll;
+						data.yaw = td.yaw;
+						data.fps = td.fps;
+						(*cb)(&data);
+}
+
+extern "C"
+{
+	struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
+			const char * server)
+	{
+		try {
+			auto inst = new struct ucart_vrpn_tracker;
+			inst->t = new microcart::Tracker(server);
+			return inst;
+		} 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 {
+			auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
+			inst->t->addCallback(new_cb);
+		} 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/backend/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 {
+	public:
+		double x;
+		double y;
+		double z;
+
+		double pitch;
+		double roll;
+		double yaw;
+
+		double fps;
+		timeval timestamp;
+
+		TrackerData();
+	};
+
+	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

groundStation/src/cli/cli.c

+#include <stdio.h>
+#include <string.h>
+#include <err.h>
+#include <libgen.h>
+
+#include "cli.h"
+
+int main(int argc, char **argv)
+{
+	int cmdID = -1;
+	char * command;
+	int i , useSymlink = 0;
+	struct backend_conn  *conn;
+
+	command = basename(argv[0]);
+	for(i = 0; i < MAX_COMMANDS; ++i) {
+		if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
+		{
+			cmdID = i;
+			useSymlink = 1;
+		}
+	}
+
+	if(cmdID == -1) {
+		command = argv[1];
+		for(i = 0; i < MAX_COMMANDS; ++i) {
+			if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
+			{
+				cmdID = i;
+			}	
+		}
+	}
+
+	if(cmdID == -1){
+		printf("Could not match input with a command. Please try again...\n");
+		return -1;
+	}
+
+	printf("Parsed Command : %s\n", commandNames[cmdID]);
+	conn = ucart_backendConnect();
+	if(conn == NULL) {
+		return -1;
+	}
+
+	if(useSymlink) {
+		(*cli_functions[cmdID]) (conn, argc, &argv[0]);
+	}else {
+		(*cli_functions[cmdID]) (conn, argc-1, &argv[1]);
+	}
+
+	ucart_backendDisconnect(conn);
+    return 0;
+}

groundStation/src/cli/cli.h

+#ifndef __CLI_H
+#define __CLI_H
+
+#include "cli_monitor.h"
+#include "cli_setpid.h"
+#include "cli_getpid.h"
+// #include "cli_getimu.h"
+enum CommandNameIds{
+	CMD_MONITOR,
+	CMD_GETPID,
+	CMD_SETPID,
+	CMD_GETIMU,
+	MAX_COMMANDS
+};
+
+typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
+static cli_function_ptr cli_functions[] = {
+	&cli_monitor,
+	&cli_getpid,
+	&cli_setpid
+	//&cli_getimu
+};
+
+static char* commandNames[MAX_COMMANDS] = {
+	"monitor",
+	"getpid",
+	"setpid",
+	"getImu"
+};
+
+
+#endif

groundStation/src/cli/cli_getimu.h

+#ifndef CLI_GETIMU_H
+#define CLI_GETIMU_H
+#include "frontend_getimu.h"
+
+int cli_getimu(
+		struct backend_conn * conn,
+		struct frontend_pid_data * pid_data);
+
+#endif
\ No newline at end of file

groundStation/src/cli/cli_getpid.c

+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include "cli_getpid.h"
+
+int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
+	int c;
+	static int getRoll = 0, getPitch = 0, getYaw = 0, getAll = 0;
+	static int getRollV = 0, getPitchV = 0, getYawV = 0;
+	static int getHeight = 0, getLat = 0, getLong = 0;
+	struct frontend_pid_data pid_data;
+	static struct option long_options[] = {
+ 		/* These options don’t set a flag. We distinguish them by their indices. */
+ 		{"roll",	no_argument,	&getRoll,	1},
+ 		{"pitch",   no_argument,   	&getPitch, 	1},
+ 		{"yaw",   no_argument,   	&getYaw, 	1},
+ 		{"rollv",   no_argument,   	&getYawV, 	1},
+ 		{"pitchv",   no_argument,   	&getYawV, 	1},
+ 		{"yawv",   no_argument,   	&getYawV, 	1},
+ 		{"height",   no_argument,   	&getHeight,	1},
+ 		{"lat",   no_argument,   	&getLat, 	1},
+ 		{"long",   no_argument,   	&getLong, 	1},
+ 		{0, 0, 0, 0}
+ 	};
+
+ 	while (1)
+	{
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "a", long_options, &option_index);
+
+		/* Detect the end of the options. */
+		if (c == -1)
+			break;
+
+		if (c == 'a') {
+			getAll = 1;
+		}
+	}
+
+	int result;
+	if(getAll) {
+		pid_data.controller = PID_ROLL;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+		pid_data.controller = PID_PITCH;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+		pid_data.controller = PID_YAW;
+		if ((result = getValues(conn, &pid_data))) {
+			return result;
+		}
+	} else {
+		if(getPitch) {
+			pid_data.controller = PID_PITCH;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getRoll) {
+			pid_data.controller = PID_ROLL;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getYaw) {
+			pid_data.controller = PID_YAW;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
+	if(frontend_getpid(conn, pid_data)) {
+		return 1;
+	}
+
+	return 0;
+
+	switch(pid_data->controller) {
+		case PID_PITCH :
+			printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_ROLL :
+			printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_YAW :
+			printf("Yaw Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		default :
+			break;
+	}
+	return 0;
+}

groundStation/src/cli/cli_getpid.h

+#ifndef CLI_GETPID_H
+#define CLI_GETPID_H
+
+#include "frontend_getpid.h"
+
+int getValues(struct backend_conn *, struct frontend_pid_data *);
+int cli_getpid(struct backend_conn * conn, int argc, char ** argv);
+
+#endif
\ No newline at end of file