diff --git a/groundStation/.gitignore b/groundStation/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..508a8f243429591a76f612c87353c847358052e0
--- /dev/null
+++ b/groundStation/.gitignore
@@ -0,0 +1,44 @@
+# 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
diff --git a/groundStation/Makefile b/groundStation/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..50c78782e80cb912a6eb2f42906b7c363e774a06
--- /dev/null
+++ b/groundStation/Makefile
@@ -0,0 +1,76 @@
+# 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)
diff --git a/groundStation/README.md b/groundStation/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..a54b1f43611542c3792549e51f1509bc187a5428
--- /dev/null
+++ b/groundStation/README.md
@@ -0,0 +1,63 @@
+# 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`
+
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
new file mode 100644
index 0000000000000000000000000000000000000000..5c4dc9fa6590757f051969825b6969c62bcf8fb5
--- /dev/null
+++ b/groundStation/src/backend/backend.c
@@ -0,0 +1,773 @@
+/* Author: Kris Burney & Jake Drahos
+ *
+ * BlueTooth socket program for passing vrpn data to quad.
+ */
+
+#define _GNU_SOURCE
+#define _BSD_SOURCE
+
+//system includes
+#include <err.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <signal.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <sys/select.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/rfcomm.h>
+#include <pthread.h>
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <netinet/tcp.h>
+
+//user created includes
+#include "communication.h"
+#include "commands.h"
+#include "vrpn_tracker.hpp"
+#include "type_def.h"
+#include "logger.h"
+#include "config.h"
+
+#define QUAD_BT_ADDR "00:06:66:64:61:D6"
+#define QUAD_BT_CHANNEL 0x01
+#define CMD_MAX_LENGTH 1024
+#define MAX_HASH_SIZE 50
+
+/* Backend-internal command magics */
+#define TD_MAGIC "TRACKERDATA"
+
+// function prototypes
+void readAndPrint(void);
+void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
+void sendStartPacket(void);
+void getVRPNPacket(struct ucart_vrpn_TrackerData *);
+void printVrpnData(struct ucart_vrpn_TrackerData *);
+int connectToZybo();
+int safe_fd_set(int , fd_set* , int* );
+int safe_fd_clr(int , fd_set* , int* );
+static void safe_close_fd(int fd, pthread_mutex_t *mutexLock);
+
+static void cb(struct ucart_vrpn_TrackerData *);
+static int new_client(int fd);
+/* Return index of client, or -1 */
+static ssize_t get_client_index(int fd);
+/* Returns pointer to client buffer, or -1 */
+static char * get_client_buffer(int fd);
+/* Return pointer to client pending responses, or -1*/
+static int * get_client_pend_responses(int fd);
+/* Return positive integer if successful, -1 otherwise */
+static int clientAddPendResponses(int fd, unsigned char *packet);
+/* Returns -1 on error */
+static int remove_client(int fd);
+/* Receive data from client */
+static void client_recv(int fd);
+/* Receive data from quad */
+static void quad_recv(const char * buf, size_t len);
+/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
+static int wasDisconnected(int fd);
+
+/* Thread-safe wrappers */
+pthread_mutex_t quadSocketMutex;
+static ssize_t writeQuad(const char * buf, size_t count);
+static ssize_t readQuad(char * buf, size_t count);
+
+/* Functions for recording Latencies */
+void findTimeDiff(int respID);
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y);
+//time stamp checking
+static unsigned int currMessageID = 0;
+struct timeval timeArr[MAX_HASH_SIZE];
+
+// global variables
+static volatile int keepRunning = 1;
+const char *TRACKER_IP = "UAV@192.168.0.120:3883";
+static int zyboSocket;
+static int backendSocket;
+struct ucart_vrpn_tracker * tracker = NULL;
+const char *logHeader = "";//"#\n#\tDefault log header\n#\tEverything after '#'`s will be printed as is in the processed logs.\n#\n\0";
+
+#define MAX_CLIENTS 32
+#define CLIENT_BUFFER_SIZE 1024
+#define CLIENT_MAX_PENDING_RESPONSES 5
+static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE];
+static int client_fds[MAX_CLIENTS];
+static int client_pending_responses[MAX_CLIENTS][CLIENT_MAX_PENDING_RESPONSES];
+fd_set rfds_master;
+int max_fd = 0;
+
+pthread_mutex_t quadResponseMutex, cliInputMutex ;
+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++;
+}
+
+int main(int argc, char **argv)
+{
+ int activity;
+ FD_ZERO(&rfds_master);
+
+ /*
+ * Create backend listening socket
+ */
+ /* Determine socket path */
+ char * backend_socket_path = DEFAULT_SOCKET;
+ if (getenv(SOCKET_ENV)) {
+ backend_socket_path = getenv(SOCKET_ENV);
+ }
+ /* Unlink if it exists */
+ unlink(backend_socket_path);
+
+ /* Create socket */
+ backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
+ if (backendSocket < 0) {
+ err(-1, "socket");
+ }
+
+ /* Create sockaddr and bind */
+ struct sockaddr_un sa;
+ sa.sun_family = AF_UNIX;
+ strncpy(sa.sun_path, backend_socket_path, 107);
+ sa.sun_path[107] = '\0';
+ if (bind(backendSocket, (struct sockaddr *) &sa, sizeof(sa))) {
+ err(-1, "bind");
+ }
+
+ /* Listen */
+ if (listen(backendSocket, 16)) {
+ err(-1, "listen");
+ }
+
+ /* Add to socket set */
+ safe_fd_set(backendSocket, &rfds_master, &max_fd);
+
+
+ /* Initialize client buffers */
+ for (int i = 0; i < MAX_CLIENTS; i++) {
+ client_fds[i] = -1;
+ client_buffers[i][0] = '\n';
+ for(int j = 0; j < CLIENT_MAX_PENDING_RESPONSES; j++) {
+ client_pending_responses[i][j] = -1;
+ }
+ }
+
+ if (pthread_mutex_lock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+
+ if ((zyboSocket = connectToZybo()) < 0)
+ {
+ perror("Error connecting to Quad...");
+ free(respBuf);
+ free(commandBuf);
+ exit(1);
+ }
+
+ if (pthread_mutex_unlock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+ }
+
+ // open the log file
+ if(createLogFile(argc, argv[1]))
+ {
+ perror("Error creating log file...");
+ exit(1);
+ }
+ // writeStringToLog(logHeader);
+
+ // watch for input from stdin (fd 0) to see when it has input
+ safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
+
+ if (!getenv(NOQUAD_ENV)) {
+ // watch for input from the zybo socket
+ safe_fd_set(zyboSocket, &rfds_master, &max_fd);
+
+ }
+ // Tell the quad we are ready to send it vrpn data
+ sendStartPacket();
+
+ if(!getenv(NOVRPN_ENV)){
+ // create vrpnTracker instance
+ tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
+ // this function will be called whenever tracker receives data
+ ucart_vrpn_tracker_addCallback(tracker, cb);
+ }
+
+ struct timeval timeout = {
+ .tv_sec = 1,
+ .tv_usec = 0
+ };
+
+ respBuf = calloc(CMD_MAX_LENGTH, sizeof(unsigned char));
+ sleep(3);
+ while(keepRunning)
+ {
+ fd_set rfds;
+ rfds = rfds_master;
+ activity = select(max_fd+1, &rfds, NULL, NULL, NULL);
+ if(activity == -1) {
+ perror("select() ");
+ } else if (activity) {
+ for(int fd = 0; fd <= max_fd; ++fd) {
+ if (FD_ISSET(fd, &rfds)) {
+ if(wasDisconnected(fd)){
+ break;
+ }
+
+ if (fd == fileno(stdin)) {
+ /**
+ * Ignore stdin from the backend
+ */
+ } else if (fd == zyboSocket) {
+ printf("recieving from quad\n");
+
+ /**
+ * Read the response from the control loop
+ */
+ int available;
+ ioctl(fd, FIONREAD, &available);
+ if (available < 12) {
+ continue;
+ }
+ int respLen = readQuad(respBuf, CMD_MAX_LENGTH);
+ if(respLen <= 0) {
+ perror("ERROR reading from quad...\n");
+ }
+
+ //int id = getInt((unsigned char *)respBuf, 7);
+ //findTimeDiff(id);
+
+ quad_recv(respBuf, respLen);
+ // if(respLen == 11) {
+ // int id = getInt((unsigned char *)respBuf, 7);
+ // findTimeDiff(id);
+ // printf("respLen = %d : id = %d'\n", respLen, id);
+ // for(int i = 0; i <= respLen -1; ++i)
+ // printf("%x ", (unsigned char)respBuf[i]);
+ // printf("'\n");
+ // }
+ memset(respBuf, 0, respLen);
+ } else if (fd == backendSocket) {
+ int new_fd = 0;
+ new_fd = accept(backendSocket, NULL, NULL);
+ if (new_fd < 0) {
+ warn("accept");
+ } else {
+ printf("Connection\n");
+ if (new_client(new_fd)) {
+ printf("Added client\n");
+ safe_fd_set(new_fd, &rfds_master, &max_fd);
+ }
+ }
+ } else if (get_client_index(fd) > -1) {
+ /* It is a socket to a frontend */
+ client_recv(fd);
+ }
+ }
+ }
+ } else {
+ timeout.tv_sec = 1;
+ timeout.tv_usec = 0;
+ }
+ }
+
+ ucart_vrpn_tracker_freeInstance(tracker);
+
+ safe_close_fd(zyboSocket, &quadSocketMutex);
+
+ closeLogFile();
+ return 0;
+}
+
+void sendStartPacket() {
+ unsigned char packet[8] = {0};
+ currMessageID++;
+ metadata_t metadata =
+ {
+ (char) BEGIN_CHAR,
+ 0x04,
+ 0x01,
+ 0x01,
+ 0
+ };
+
+ packet[0] = metadata.begin_char; // BEGIN //PACKET_START_BYTE;
+ packet[1] = metadata.msg_type; // UPDATE //'U'; // U for vrpn camera update, C for command
+ packet[2] = metadata.msg_subtype; // BEGIN UPDATE
+ packet[3] = (currMessageID & 0x000000ff); // MSG ID(1)
+ packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
+ packet[5] = 0; // DATALEN(1)
+ packet[6] = 0; // DATALEN(2)
+
+ char checksum = 0;
+ int i;
+ for(i=0; i < metadata.data_len + 7; i++)
+ checksum ^= packet[i];
+
+ packet[metadata.data_len + 7] = checksum; //PACKET_END_BYTE;
+ int status = writeQuad((char * ) packet, metadata.data_len + 8);
+ if (status != 8)
+ {
+ perror("Error sending start packet...\n");
+ keepRunning = 0;
+ }else
+ {
+ printf("Start packet successfuly sent...\n");
+ }
+}
+
+void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
+ int pSize = sizeof(struct ucart_vrpn_TrackerData) + 8;
+ int n;
+ unsigned char packet[pSize];
+ currMessageID++;
+ packet[0] = 0xBE; // BEGIN //PACKET_START_BYTE;
+ packet[1] = 0x04; // UPDATE //'U'; // U for vrpn camera update, C for command
+ packet[2] = 0x00; // N/A
+ //TODO Figure out Packet ID with this new ucar_vrpn_TrackerData struct
+ packet[3] = (currMessageID & 0x000000ff); // MSG ID(1)
+ packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
+ packet[5] = (sizeof(struct ucart_vrpn_TrackerData) & 0x000000ff); // DATALEN(1)
+ packet[6] = ((sizeof(struct ucart_vrpn_TrackerData) >> 8) & 0x00000ff); // DATALEN(2)
+ memcpy(&packet[7], &info, sizeof(struct ucart_vrpn_TrackerData));
+
+ char checksum = 0;
+ int i;
+ for(i=0; i < pSize - 1; i++)
+ checksum ^= packet[i];
+
+ packet[pSize - 1] = checksum; //PACKET_END_BYTE;
+
+ n = writeQuad((char *) packet, pSize);
+ if(n < 0) {
+ perror("vrpnhandler: ERROR writing to socket");
+ keepRunning = 0;
+ }
+ struct timeval tstart;
+ gettimeofday(&tstart, NULL);
+ timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
+}
+
+void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
+ int status;
+ if((status = ucart_vrpn_tracker_getData(tracker, td)) < 0)
+ {
+ perror("Error receiving VRPN data from tracker...");
+ keepRunning = 0;
+ }
+}
+
+void printVrpnData(struct ucart_vrpn_TrackerData * td) {
+ printf("FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
+ td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
+}
+
+int connectToZybo() {
+ int sock;
+ int status = 0;
+
+ if (getenv(NOQUAD_ENV)) {
+ return 0;
+ }
+
+ /* Use bluetooth by default */
+ if (!getenv(QUAD_WIFI_ENV)) {
+ printf("Using BT Settings\n");
+ struct sockaddr_rc addr;
+
+ // allocate a socket
+ sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+
+ //set the connection params ie. who to connect to
+ addr.rc_family = AF_BLUETOOTH;
+ addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
+ str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
+
+ printf("Attempting to connect to zybo. Please be patient...\n");
+ // blocking call to connect to socket sock ie. zybo board
+ status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+ } else {
+ printf("Using WIFI settings\n");
+ struct sockaddr_in addr;
+
+ addr.sin_family = AF_INET;
+
+ /* Quick and Dirty */
+ if (getenv(QUAD_IP_ENV)) {
+ if (!inet_aton(getenv(QUAD_IP_ENV), &addr.sin_addr)) {
+ printf("Env var %s invalid IP %s\n",
+ QUAD_IP_ENV, getenv(QUAD_IP_ENV));
+ return -1;
+ }
+ } else {
+ if (!inet_aton(QUAD_IP_DEFAULT, &addr.sin_addr)) {
+ printf("Default IP %s is invalid\n",
+ QUAD_IP_DEFAULT);
+ return -1;
+ }
+ }
+
+ if (getenv(QUAD_PORT_ENV)) {
+ /* Quick 'n dirty, oh yeah! */
+ addr.sin_port = htons(atoi(getenv(QUAD_PORT_ENV)));
+ } else {
+ addr.sin_port = htons(QUAD_PORT_DEFAULT);
+ }
+
+ sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
+ if (sock < 0) {
+ perror("socket");
+ return -1;
+ }
+ printf("Connecting to Quad @ %s:%u\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port));
+
+ status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+ }
+
+ // connection failed
+ if(status < 0)
+ {
+ close(sock);
+ perror("connect");
+ return -1;
+ }
+ else
+ {
+ // int result = setsockopt(sock, IPPROTO_TCP, TCP_NODELAY, (int[]){1}, sizeof(int));
+ // printf("result = %d\n", result);
+ printf("connection successful!...\n");
+ return sock;
+ }
+}
+
+/* add a fd to fd_set, and update max_fd */
+int safe_fd_set(int fd, fd_set* fds, int* max_fd) {
+ assert(max_fd != NULL);
+
+ FD_SET(fd, fds);
+ if (fd > *max_fd) {
+ *max_fd = fd;
+ }
+ return 0;
+}
+
+/* clear fd from fds, update max fd if needed */
+int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
+ assert(max_fd != NULL);
+
+ FD_CLR(fd, fds);
+ if (fd == *max_fd) {
+ (*max_fd)--;
+ }
+ return 0;
+}
+
+static ssize_t writeQuad(const char * buf, size_t count) {
+ ssize_t retval;
+ if (getenv(NOQUAD_ENV)) {
+ return count;
+ }
+ if (pthread_mutex_lock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+ //setsockopt(zyboSocket, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int));
+ retval = write(zyboSocket, buf, count);
+ if (pthread_mutex_unlock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+ }
+
+ return retval;
+}
+
+static ssize_t readQuad(char * buf, size_t count) {
+ ssize_t retval;
+ if (pthread_mutex_lock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+ retval = read(zyboSocket, buf, count);
+ //int result = setsockopt(zyboSocket, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int));
+ //printf("result = %d\n", result);
+
+ if (pthread_mutex_unlock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+ }
+ return retval;
+}
+
+static int new_client(int fd) {
+ ssize_t new_slot = -1;
+ for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
+ if (client_fds[i] < 0) {
+ new_slot = i;
+ break;
+ }
+ }
+ if (new_slot == -1) {
+ warnx("Ran out of room! Consider increasing MAX_CLIENTS!");
+ return 0;
+ }
+
+ client_fds[new_slot] = fd;
+ client_buffers[new_slot][0] = '\0';
+
+ return 1;
+}
+
+static ssize_t get_client_index(int fd) {
+ for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
+ if (client_fds[i] == fd) {
+ return i;
+ }
+ }
+
+ return -1;
+}
+
+static char * get_client_buffer(int fd) {
+ ssize_t slot = get_client_index(fd);
+ if (slot == -1) {
+ return NULL;
+ } else {
+ return client_buffers[slot];
+ }
+}
+
+static int * get_client_pend_responses(int fd) {
+ ssize_t slot = get_client_index(fd);
+ if (slot == -1) {
+ return NULL;
+ } else {
+ return client_pending_responses[slot];
+ }
+}
+
+static int clientAddPendResponses(int fd, unsigned char *packet) {
+ int *pendingResponses = get_client_pend_responses(fd);
+ int packetID = (packet[4] << 8) | (packet[3]);
+ for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
+ if(pendingResponses[i] == -1) {
+ pendingResponses[i] = packetID;
+ return i;
+ }
+ }
+ return -1;
+}
+
+static int remove_client(int fd) {
+ ssize_t slot = get_client_index(fd);
+ if(slot == -1)
+ return -1;
+ char *clientBuffer = get_client_buffer(fd);
+ if(clientBuffer == NULL)
+ return -1;
+ clientBuffer[0] = '\0';
+ int *pendingResponses = get_client_pend_responses(fd);
+ if(pendingResponses == NULL)
+ return -1;
+ for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
+ pendingResponses[i] = -1;
+ }
+ client_fds[slot] = -1;
+ return 0;
+}
+
+static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
+ if (pthread_mutex_lock(mutexLock)) {
+ err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+ close(fd);
+ if (pthread_mutex_unlock(mutexLock)) {
+ err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+ }
+}
+
+static void client_recv(int fd) {
+ char * buffer;
+ ssize_t len_pre;
+ buffer = get_client_buffer(fd);
+ len_pre = strlen(buffer);
+
+ char * cursor;
+ cursor = buffer + len_pre;
+
+ ssize_t r;
+ r = read(fd, cursor, CLIENT_BUFFER_SIZE - len_pre - 1);
+ if (r < 0) {
+ warn("read (fd: %d)", fd);
+ }
+ buffer[len_pre + r] = '\0';
+
+ int index = 0;
+ /* Parse buffer and handle commands */
+ while (1) {
+ /* not using strtok because reasons */
+ size_t len = strlen(buffer);
+ ssize_t newline = -1;
+ for (size_t i = 0; i < len; i++) {
+ if (buffer[i] == '\n') {
+ newline = i;
+ break;
+ }
+ }
+
+ /* No newline found. End parsing */
+ if (newline == -1) {
+ break;
+ }
+ buffer[newline] = '\0';
+
+ printf("Client(%d) : '%s'\n",fd, buffer);
+ unsigned char * packet;
+ if(formatCommand(buffer, &packet) == -1) {
+ /* buffer was not a quad command, handling internally to
+ * backend instead of forwarding to quad
+ */
+ if (strncmp(buffer, TD_MAGIC, strlen(TD_MAGIC)) == 0) {
+ /* Request for tracker data */
+ struct ucart_vrpn_TrackerData td;
+ if (tracker == NULL) {
+ char * dummy = TD_MAGIC " 1.0 2.0 3.0 4.0 5.0 6.0\n";
+ write(fd, dummy, strlen(dummy));
+ }else if (ucart_vrpn_tracker_getData(tracker, &td)) {
+ write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
+ } else {
+ /* more than sufficient buffer */
+ char buffer[2048];
+ /* Map VRPN XYZ to Height Lat Long (right now it's
+ * a guess). Format is Height Lat Long P R Y */
+ if (snprintf(buffer,
+ 2048,
+ TD_MAGIC " %lf %lf %lf %lf %lf %lf\n",
+ td.z,
+ td.y,
+ td.x,
+ td.pitch,
+ td.roll,
+ td.yaw) >= 2048) {
+
+ /* Output longer than buffer */
+ warnx("Increase format buffer size, output was too long!");
+ write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
+ }
+ write(fd, buffer, strlen(buffer));
+ }
+ }
+ } else {
+ if(clientAddPendResponses(fd, packet) == -1) {
+ warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
+ } else {
+ int datalen = (packet[6] << 8) | (packet[5]);
+ printf("sending %lf '", getFloat(packet, 7));
+ for(int i = 0; i < datalen + 8; ++i) {
+ printf(" 0x%x", (signed) packet[i]);
+ }
+ printf("'\n");
+ writeQuad((char *) packet, datalen +8);
+ }
+ }
+
+ char * rest = &buffer[newline] + 1;
+ size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
+ /* Delete parsed data and move the rest to the left */
+ memmove(buffer, rest, restLen +1);
+ }
+}
+
+static void quad_recv(const char * buf, size_t len) {
+ /* Check to see which command we are receiving. If it is one that needs to be passed on
+ onto the clients, do so.
+ */
+
+ char packet[len];
+ int validPacket;
+ unsigned char *data;
+ metadata_t metadata;
+
+ memcpy(packet, buf, len);
+
+ // Validate the message is correctly formatted
+ validPacket = parse_packet((unsigned char *) packet, &data, &metadata);
+ if(validPacket != 0) {
+ warnx("Could not recognize packet from quad.\n");
+ return;
+ }
+
+ int datalen = (packet[6] << 8) | (packet[5]);
+ char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText;
+ float value = getFloat((unsigned char *)packet, 7);
+ printf("Quad : %s, %lf\n", cmdText, value);
+ /*
+ Assuming the quad sends the correct info.. This hasn't been tested yet due to a lack of
+ quad software. We can check how to format by the cmdText and pass to every client.
+ */
+ char buffer[1048];
+ sprintf(buffer, "%s %lf\n", cmdText, value);
+
+ for(int fd = 0; fd <= max_fd; ++fd) {
+ if (get_client_index(fd) > -1) {
+ write(fd, buffer, datalen + 8);
+ }
+ }
+}
+
+static int wasDisconnected(int fd) {
+ char buff;
+ if(recv(fd, &buff, 1, MSG_PEEK | MSG_DONTWAIT) == 0)
+ {
+ remove_client(fd);
+ safe_fd_clr(fd, &rfds_master, &max_fd);
+ printf("fd %d has disconnect and was removed\n", fd);
+ return 1;
+ }
+ return 0;
+}
+
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y) {
+ /* Perform the carry for the later subtraction by updating y. */
+ if (x->tv_usec < y->tv_usec) {
+ int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
+ y->tv_usec -= 1000000 * nsec;
+ y->tv_sec += nsec;
+ }
+ if (x->tv_usec - y->tv_usec > 1000000) {
+ int nsec = (x->tv_usec - y->tv_usec) / 1000000;
+ y->tv_usec += 1000000 * nsec;
+ y->tv_sec -= nsec;
+ }
+ /* Compute the time remaining to wait.
+ tv_usec is certainly positive. */
+ result->tv_sec = x->tv_sec - y->tv_sec;
+ result->tv_usec = x->tv_usec - y->tv_usec;
+ /* Return 1 if result is negative. */
+ return x->tv_sec < y->tv_sec;
+}
+
+void findTimeDiff(int respID) {
+ struct timeval result, tend;
+ gettimeofday(&tend, NULL);
+ timeval_subtract(&result, &tend, &timeArr[respID%MAX_HASH_SIZE]);
+ printf("elapsed time = %ld ms\n", result.tv_usec/1000);
+ // char tmp[8];
+ // snprintf(tmp, 8, "%ld \tms\n", result.tv_usec/1000);
+ // writeStringToLog(tmp);
+}
diff --git a/groundStation/src/backend/commands.c b/groundStation/src/backend/commands.c
new file mode 100644
index 0000000000000000000000000000000000000000..a97197b8106334e13b2e652ab93c1228f868ce0b
--- /dev/null
+++ b/groundStation/src/backend/commands.c
@@ -0,0 +1,1150 @@
+#include "commands.h"
+
+// TAKE THESE OUT WHEN IMPLEMENTING ON THE QUAD SIDE
+float getFloat(unsigned char* str, int pos) {
+ union {
+ float f;
+ int i;
+ } x;
+ x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
+ return x.f;
+}
+
+int getInt(unsigned char* str, int pos) {
+ int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
+ return i;
+}
+//------------------------------------------------
+
+struct MessageType MessageTypes[MAX_TYPE] =
+{
+ // DEBUG
+ {
+ // Message Type ID
+ 0x00,
+
+ // Debug Subtypes
+ {
+ // NONE subtype
+ {
+ // ID
+ 0x00,
+ // Command text
+ "debug",
+ // Type of the command data
+ stringType,
+ // Function pointer
+ &debug
+ }
+ }
+ },
+
+ // CALIBRATION
+ {
+ // Message Type ID
+ 0x01,
+
+ // Calibration Subtypes (PID coefficients)
+ {
+ // yaw p constant subtype
+ {
+ // ID
+ 0x00,
+ // Command text
+ "setyawp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // yaw i constant subtype
+ {
+ // ID
+ 0x01,
+ // Command text
+ "setyawi",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // yaw d constant subtype
+ {
+ // ID
+ 0x02,
+ // Command text
+ "setyawd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // roll p constant subtype
+ {
+ // ID
+ 0x03,
+ // Command text
+ "setrollp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // roll i constant subtype
+ {
+ // ID
+ 0x04,
+ // Command text
+ "setrolli",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // roll d constant subtype
+ {
+ // ID
+ 0x05,
+ // Command text
+ "setrolld",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // pitch p constant subtype
+ {
+ // ID
+ 0x06,
+ // Command text
+ "setpitchp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // pitch i constant subtype
+ {
+ // ID
+ 0x07,
+ // Command text
+ "setpitchi",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // pitch d constant subtype
+ {
+ // ID
+ 0x08,
+ // Command text
+ "setpitchd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // yawrate p constant subtype
+ {
+ // ID
+ 0x09,
+ // Command text
+ "setyawratep",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // yawrate i constant subtype
+ {
+ // ID
+ 0x0A,
+ // Command text
+ "setyawratei",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // yawrate d constant subtype
+ {
+ // ID
+ 0x0B,
+ // Command text
+ "setyawrated",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // rollrate p constant subtype
+ {
+ // ID
+ 0x0C,
+ // Command text
+ "setrollratep",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // rollrate i constant subtype
+ {
+ // ID
+ 0x0D,
+ // Command text
+ "setrollratei",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // rollrate d constant subtype
+ {
+ // ID
+ 0x0E,
+ // Command text
+ "setrollrated",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // pitchrate p constant subtype
+ {
+ // ID
+ 0x0F,
+ // Command text
+ "setpitchratep",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // pitchrate i constant subtype
+ {
+ // ID
+ 0x10,
+ // Command text
+ "setpitchratei",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // pitchrate d constant subtype
+ {
+ // ID
+ 0x11,
+ // Command text
+ "setpitchrated",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // height p constant subtype
+ {
+ // ID
+ 0x12,
+ // Command text
+ "setheightp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // height i constant subtype
+ {
+ // ID
+ 0x13,
+ // Command text
+ "setheighti",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // height d constant subtype
+ {
+ // ID
+ 0x14,
+ // Command text
+ "setheightd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // lat p constant subtype
+ {
+ // ID
+ 0x15,
+ // Command text
+ "setlatp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // lat i constant subtype
+ {
+ // ID
+ 0x16,
+ // Command text
+ "setlati",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // lat d constant subtype
+ {
+ // ID
+ 0x17,
+ // Command text
+ "setlatd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // long p constant subtype
+ {
+ // ID
+ 0x18,
+ // Command text
+ "setlongp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // long i constant subtype
+ {
+ // ID
+ 0x19,
+ // Command text
+ "setlongi",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ },
+ // long d constant subtype
+ {
+ // ID
+ 0x1A,
+ // Command text
+ "setlongd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ NULL
+ }
+ }
+ },
+
+ // REQUEST
+ {
+ // Message Type ID
+ 0x02,
+
+ // Request Subtypes
+ {
+ // accelerometer subtype
+ {
+ // ID
+ 0x00,
+ // Command text
+ "getaccel",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getaccel
+ },
+ // gyroscope subtype
+ {
+ // ID
+ 0x01,
+ // Command text
+ "getgyro",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getgyro
+ },
+ // pitch angle subtype
+ {
+ // ID
+ 0x02,
+ // Command text
+ "getpitchangle",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getpitchangle
+ },
+ // roll angle subtype
+ {
+ // ID
+ 0x03,
+ // Command text
+ "getrollangle",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getrollangle
+ },
+ // get yaw setpoint subtype
+ {
+ // ID
+ 0x04,
+ // Command text
+ "getyaw",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &setyaw
+ },
+ // get yaw p constant subtype
+ {
+ // ID
+ 0x05,
+ // Command text
+ "getyawp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getyawp
+ },
+ // get yaw d constant subtype
+ {
+ // ID
+ 0x06,
+ // Command text
+ "getyawd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getyawd
+ },
+ // get roll setpoint subtype
+ {
+ // ID
+ 0x07,
+ // Command text
+ "getroll",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getroll
+ },
+ // get roll p constant subtype
+ {
+ // ID
+ 0x08,
+ // Command text
+ "getrollp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getrollp
+ },
+ // getroll d constant subtype
+ {
+ // ID
+ 0x09,
+ // Command text
+ "getrolld",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getrolld
+ },
+ // get pitch setpoint subtype
+ {
+ // ID
+ 0x0A,
+ // Command text
+ "getpitch",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getpitch
+ },
+ // get pitch p constant subtype
+ {
+ // ID
+ 0x0B,
+ // Command text
+ "getpitchp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getpitchp
+ },
+ // get pitch d constant subtype
+ {
+ // ID
+ 0x0C,
+ // Command text
+ "getpitchd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getpitchd
+ },
+ // get throttle setpoint subtype
+ {
+ // ID
+ 0x0D,
+ // Command text
+ "getthrottle",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getthrottle
+ },
+ // get throttle p constant subtype
+ {
+ // ID
+ 0x0E,
+ // Command text
+ "getthrottlep",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getthrottlep
+ },
+ // get throttle i constant subtype
+ {
+ // ID
+ 0x0F,
+ // Command text
+ "getthrottlei",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getthrottlei
+ },
+ // get throttle d constant subtype
+ {
+ // ID
+ 0x10,
+ // Command text
+ "getthrottled",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &getthrottled
+ }
+ }
+ },
+
+ // RESPONSE
+ {
+ // Message Type ID
+ 0x03,
+
+ // Response Subtypes
+ {
+ // accelerometer subtype
+ {
+ // ID
+ 0x00,
+ // Command text
+ "respaccel",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respaccel
+ },
+ // gyroscope subtype
+ {
+ // ID
+ 0x01,
+ // Command text
+ "respgyro",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respgyro
+ },
+ // pitch angle subtype
+ {
+ // ID
+ 0x02,
+ // Command text
+ "resppitchangle",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resppitchangle
+ },
+ // roll angle subtype
+ {
+ // ID
+ 0x03,
+ // Command text
+ "resprollangle",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resprollangle
+ },
+ // resp yaw setpoint subtype
+ {
+ // ID
+ 0x04,
+ // Command text
+ "respyaw",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &setyaw
+ },
+ // resp yaw p constant subtype
+ {
+ // ID
+ 0x05,
+ // Command text
+ "respyawp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respyawp
+ },
+ // resp yaw d constant subtype
+ {
+ // ID
+ 0x06,
+ // Command text
+ "respyawd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respyawd
+ },
+ // resp roll setpoint subtype
+ {
+ // ID
+ 0x07,
+ // Command text
+ "resproll",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resproll
+ },
+ // resp roll p constant subtype
+ {
+ // ID
+ 0x08,
+ // Command text
+ "resprollp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resprollp
+ },
+ // resproll d constant subtype
+ {
+ // ID
+ 0x09,
+ // Command text
+ "resprolld",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resprolld
+ },
+ // resp pitch setpoint subtype
+ {
+ // ID
+ 0x0A,
+ // Command text
+ "resppitch",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resppitch
+ },
+ // resp pitch p constant subtype
+ {
+ // ID
+ 0x0B,
+ // Command text
+ "resppitchp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resppitchp
+ },
+ // resp pitch d constant subtype
+ {
+ // ID
+ 0x0C,
+ // Command text
+ "resppitchd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resppitchd
+ },
+ // resp throttle setpoint subtype
+ {
+ // ID
+ 0x0D,
+ // Command text
+ "respthrottle",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respthrottle
+ },
+ // resp throttle p constant subtype
+ {
+ // ID
+ 0x0E,
+ // Command text
+ "respthrottlep",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respthrottlep
+ },
+ // resp throttle i constant subtype
+ {
+ // ID
+ 0x0F,
+ // Command text
+ "respthrottlei",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respthrottlei
+ },
+ // resp throttle d constant subtype
+ {
+ // ID
+ 0x10,
+ // Command text
+ "respthrottled",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respthrottled
+ }
+ }
+ },
+
+ // UPDATE
+ {
+ // Message Type ID
+ 0x04,
+
+ // Update Subtypes
+ {
+ // NONE subtype
+ {
+ // ID
+ 0x00,
+ // Command text
+ "update",
+ // Type of the command data
+ stringType,
+ // Function pointer
+ &update
+ }
+ }
+ },
+
+ // LOG
+ {
+ // Message Type ID
+ 0x05,
+
+ // Log Subtypes
+ {
+ // NONE subtype
+ {
+ // ID
+ 0x00,
+ // Command text
+ "log",
+ // Type of the command data
+ stringType,
+ // Function pointer
+ &logdata
+ },
+ // Response subtype
+ {
+ // ID
+ 0x01,
+ // Command text
+ "response",
+ // Type of the command data
+ stringType,
+ // Function pointer
+ &response
+ }
+ }
+ },
+
+};
+
+int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for debug\n");
+ return 0;
+}
+
+int update(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ unsigned char update[28];
+ memcpy(update, ((float *)packet), 28);
+
+ int packetId = getInt(update, 0);
+ float y_pos = getFloat(update, 4);
+ float x_pos = getFloat(update, 8);
+ float alt_pos = getFloat(update, 12);
+ float roll = getFloat(update, 16);
+ float pitch = getFloat(update, 20);
+ float yaw = getFloat(update, 24);
+
+ structs->log_struct.currentQuadPosition.packetId = packetId;
+ structs->log_struct.currentQuadPosition.y_pos = y_pos;
+ structs->log_struct.currentQuadPosition.x_pos = x_pos;
+ structs->log_struct.currentQuadPosition.alt_pos = alt_pos;
+ structs->log_struct.currentQuadPosition.roll = roll;
+ structs->log_struct.currentQuadPosition.pitch = pitch;
+ structs->log_struct.currentQuadPosition.yaw = yaw;
+
+ printf("QUAD: VRPN Packet:");
+ printf("Packet ID: %d\n", packetId);
+ printf("Y Position: %f\n", y_pos);
+ printf("X Position: %f\n", x_pos);
+ printf("Altitude Position: %f\n", alt_pos);
+ printf("Roll: %f\n", roll);
+ printf("Pitch: %f\n", pitch);
+ printf("Yaw: %f\n", yaw);
+
+ printf("function for update\n");
+ return 0;
+}
+
+// Why is this here?
+// This should be on the ground station side
+int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("Logging: %s\n", packet);
+ return 0;
+}
+
+int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("This is the response: %s\n", packet);
+
+ return 0;
+}
+
+// ------------------------------------------------------------------
+
+int setyaw(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+
+ printf("%f\n", value);
+
+ structs->setpoint_struct.desiredQuadPosition.yaw = value;
+
+ printf("function for setyaw: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+
+ return 0;
+}
+
+int setyawp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.yaw_angle_pid.Kp = value;
+
+ printf("function for setyawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
+
+ return 0;
+}
+
+int setyawd(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.yaw_angle_pid.Kd = value;
+
+ printf("function for setyawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
+
+ return 0;
+}
+
+int setroll(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->setpoint_struct.desiredQuadPosition.roll = value;
+
+ printf("function for setroll: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
+
+ return 0;
+}
+
+int setrollp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.local_y_pid.Kp = value;
+
+ printf("function for setrollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
+
+ return 0;
+}
+
+int setrolld(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.local_y_pid.Kd = value;
+
+ printf("function for setrolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
+
+ return 0;
+}
+
+int setpitch(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->setpoint_struct.desiredQuadPosition.pitch = value;
+
+ printf("function for setpitch: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
+
+ return 0;
+}
+
+int setpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.local_x_pid.Kp = value;
+
+ printf("function for setpitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
+
+ return 0;
+}
+
+int setpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.local_x_pid.Kd = value;
+
+ printf("function for setpitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
+
+ return 0;
+}
+
+// ------------------------------------------------------------
+// These should be renamed to altitude!
+int setthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
+
+ printf("function for setthrottle: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
+
+ return 0;
+}
+
+int setthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.alt_pid.Kp = value;
+
+ printf("function for setthrottlep: %f\n", structs->parameter_struct.alt_pid.Kp);
+
+ return 0;
+}
+
+int setthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.alt_pid.Ki = value;
+
+ printf("function for setthrottlei: %f\n", structs->parameter_struct.alt_pid.Ki);
+
+ return 0;
+}
+
+int setthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ float value;
+
+ memcpy(&value, ((float *)packet), dataLen);
+ structs->parameter_struct.alt_pid.Kd = value;
+
+ printf("function for setthrottled: %f\n", structs->parameter_struct.alt_pid.Kd);
+
+ return 0;
+}
+int getyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
+ return 0;
+}
+int getroll(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getrollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getrolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getpitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int getthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+
+// These should be renamed to altitude!
+// ------------------------------------------------------------
+
+
+
+
+int getgyro(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for getgyro\n");
+ return 0;
+}
+
+int getpitchangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for getpitchangle\n");
+ return 0;
+}
+
+int getrollangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for getrollangle\n");
+ return 0;
+}
+
+
+int getaccel(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for getaccel\n");
+ return 0;
+}
+
+
+int respgyro(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for respgyro\n");
+ return 0;
+}
+
+int resppitchangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for resppitchangle\n");
+ return 0;
+}
+
+int resprollangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for resprollangle\n");
+ return 0;
+}
+
+
+int respaccel(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for respaccel\n");
+ return 0;
+}
+
+int respyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int respyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int respyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
+ return 0;
+}
+int resproll(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int resprollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int resprolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int resppitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int resppitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int resppitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int respthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int respthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int respthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
+int respthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
+ return 0;
+}
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
new file mode 100644
index 0000000000000000000000000000000000000000..b9c9956ea0150c81291f93d3a3c665d07be67295
--- /dev/null
+++ b/groundStation/src/backend/commands.h
@@ -0,0 +1,105 @@
+#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
diff --git a/groundStation/src/backend/communication.c b/groundStation/src/backend/communication.c
new file mode 100644
index 0000000000000000000000000000000000000000..2dacd03f90afb8ed35b99a56f34ca785225e3350
--- /dev/null
+++ b/groundStation/src/backend/communication.c
@@ -0,0 +1,271 @@
+#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;
+}
diff --git a/groundStation/src/backend/communication.h b/groundStation/src/backend/communication.h
new file mode 100644
index 0000000000000000000000000000000000000000..746b9f5d45ce9199588ff45b58f5bdaa5d669ad3
--- /dev/null
+++ b/groundStation/src/backend/communication.h
@@ -0,0 +1,19 @@
+#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
diff --git a/groundStation/src/backend/config.h b/groundStation/src/backend/config.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf88e6e2b7cf09db273dba3ecc5bcf3cee7d2211
--- /dev/null
+++ b/groundStation/src/backend/config.h
@@ -0,0 +1,20 @@
+#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
diff --git a/groundStation/src/backend/logger.c b/groundStation/src/backend/logger.c
new file mode 100644
index 0000000000000000000000000000000000000000..028cfeac16c173e59154a1c9c8321eeba29d0344
--- /dev/null
+++ b/groundStation/src/backend/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/backend/logger.h b/groundStation/src/backend/logger.h
new file mode 100644
index 0000000000000000000000000000000000000000..edfb365ffce6606e7b5bd8d1f4305c5f697180a9
--- /dev/null
+++ b/groundStation/src/backend/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/backend/old_main.cold b/groundStation/src/backend/old_main.cold
new file mode 100644
index 0000000000000000000000000000000000000000..5234bcc567a6e574b30eea671b3f977a7211f478
--- /dev/null
+++ b/groundStation/src/backend/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/backend/type_def.h b/groundStation/src/backend/type_def.h
new file mode 100644
index 0000000000000000000000000000000000000000..55dca9e8fb27c26397b00961b7dbec8bbe0d4136
--- /dev/null
+++ b/groundStation/src/backend/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/backend/vrpn_tracker.cpp b/groundStation/src/backend/vrpn_tracker.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f9b56a9909ac1512d6d2164d313214200645e22
--- /dev/null
+++ b/groundStation/src/backend/vrpn_tracker.cpp
@@ -0,0 +1,178 @@
+#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;
+ }
+ }
+}
diff --git a/groundStation/src/backend/vrpn_tracker.hpp b/groundStation/src/backend/vrpn_tracker.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d10ad6e79a0bd1a3db42d0c18b4b6b286146538a
--- /dev/null
+++ b/groundStation/src/backend/vrpn_tracker.hpp
@@ -0,0 +1,100 @@
+/* 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
diff --git a/groundStation/src/cli/cli.c b/groundStation/src/cli/cli.c
new file mode 100644
index 0000000000000000000000000000000000000000..58fe14b8c9ac71026860ae4c7687ae31fa38ea26
--- /dev/null
+++ b/groundStation/src/cli/cli.c
@@ -0,0 +1,53 @@
+#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;
+}
diff --git a/groundStation/src/cli/cli.h b/groundStation/src/cli/cli.h
new file mode 100644
index 0000000000000000000000000000000000000000..f4c2fe3f4783b9b4a4dd1e3482cdf3034cbaf269
--- /dev/null
+++ b/groundStation/src/cli/cli.h
@@ -0,0 +1,32 @@
+#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
diff --git a/groundStation/src/cli/cli_getimu.h b/groundStation/src/cli/cli_getimu.h
new file mode 100644
index 0000000000000000000000000000000000000000..3f69e4b154b9678845b7604a7d91c6b83bf72ee5
--- /dev/null
+++ b/groundStation/src/cli/cli_getimu.h
@@ -0,0 +1,9 @@
+#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
diff --git a/groundStation/src/cli/cli_getpid.c b/groundStation/src/cli/cli_getpid.c
new file mode 100644
index 0000000000000000000000000000000000000000..bb887ca24242fb05f4a22710becf248cdb6f8f62
--- /dev/null
+++ b/groundStation/src/cli/cli_getpid.c
@@ -0,0 +1,105 @@
+#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;
+}
diff --git a/groundStation/src/cli/cli_getpid.h b/groundStation/src/cli/cli_getpid.h
new file mode 100644
index 0000000000000000000000000000000000000000..f5c0b15e4a2f2c7c5ad605a389b0c1b52a1344a6
--- /dev/null
+++ b/groundStation/src/cli/cli_getpid.h
@@ -0,0 +1,9 @@
+#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
diff --git a/groundStation/src/cli/cli_monitor.c b/groundStation/src/cli/cli_monitor.c
new file mode 100644
index 0000000000000000000000000000000000000000..4494e65b08320fb4831a733ff83d2bc53e803ae0
--- /dev/null
+++ b/groundStation/src/cli/cli_monitor.c
@@ -0,0 +1,102 @@
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+#include <time.h>
+#include <unistd.h>
+#include <err.h>
+
+#include "cli_monitor.h"
+#include "frontend_tracker.h"
+
+int cli_monitor(struct backend_conn * conn, int argc, char **argv) {
+ int c, result;
+ int countFlag = 0;
+
+ int count, rate = 10;
+ int forever = 0;
+
+
+ while ((c = getopt(argc, argv, "fc:r:")) != -1) {
+ switch(c) {
+ case 'c' :
+ count = atoi(optarg);
+ countFlag = 1;
+ break;
+ case 'r' :
+ rate = atoi(optarg) + 1;
+ break;
+ case 'f' :
+ forever = 1;
+ break;
+ default :
+ break;
+ }
+ }
+
+ if (forever) {
+ for (;;) {
+ struct timespec req;
+ if (rate == 1) {
+ req.tv_sec = 1;
+ req.tv_nsec = 0;
+ } else {
+ req.tv_sec = 0;
+ req.tv_nsec = 1000000000 / rate;
+ }
+ nanosleep(&req, NULL);
+ monitor(conn);
+ }
+ } else if (countFlag) {
+ for (int i = 0; i < count; i++) {
+ result = monitor(conn);
+
+ struct timespec req;
+ if (rate == 1) {
+ req.tv_sec = 1;
+ req.tv_nsec = 0;
+ } else {
+ req.tv_sec = 0;
+ req.tv_nsec = 1000000000 / rate;
+ }
+ nanosleep(&req, NULL);
+ }
+ } else {
+ return monitor(conn);
+ }
+
+ return result;
+}
+
+int monitor(struct backend_conn * conn) {
+ /* Get tracker data */
+ struct frontend_tracker_data td;
+ if (frontend_track(conn, &td)) {
+ errx(1, "Error reading tracker data");
+ }
+
+ /* TODO: Get PID constants and status */
+ /* It might be a good idea to only read the pid constants
+ * every few seconds, so count iterations and only do it if
+ * this is every (rate * 2 or 3) pass through the loop
+ */
+
+ /* Print stuff on screen */
+ /* Assuming a tab width of 8 columns */
+ printf("\033[2J");
+ printf("STATUS: NA\n");
+ printf("CTRLR :\tP\tR\tY\tP_V\tR_V\tY_V\tH\tLAT\tLON\n");
+ printf(" P :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+ printf(" I :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+ printf(" D :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+ printf("PosAtt:\tH\tLAT\tLON\tP\tR\tY\n");
+ printf(" :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+ td.height, td.lateral, td.longitudinal,
+ td.pitch, td.roll, td.yaw);
+
+ return 0;
+}
diff --git a/groundStation/src/cli/cli_monitor.h b/groundStation/src/cli/cli_monitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..308721bb89f177da345269971a8d63b32d9dc39c
--- /dev/null
+++ b/groundStation/src/cli/cli_monitor.h
@@ -0,0 +1,14 @@
+#ifndef CLI_MONITOR_H
+#define CLI_MONITOR_H
+
+#include <time.h>
+
+#include "frontend_getpid.h"
+
+#define SECOND_IN_NANO 1000000000
+
+int cli_monitor(struct backend_conn * conn, int argc, char **argv);
+
+// Return 0 on success and 1 otherwise
+int monitor(struct backend_conn * conn);
+#endif
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_setpid.c b/groundStation/src/cli/cli_setpid.c
new file mode 100644
index 0000000000000000000000000000000000000000..2fdc16fdb43e6e42ea5eea16153d98eeb5978302
--- /dev/null
+++ b/groundStation/src/cli/cli_setpid.c
@@ -0,0 +1,81 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include "cli_setpid.h"
+#include "frontend_setpid.h"
+
+int cli_setpid(struct backend_conn * conn, int argc, char **argv) {
+ int c;
+ static int setRoll = 0, setPitch = 0, setYaw = 0, setAll = 0;
+ static int setRollV = 0, setPitchV = 0, setYawV = 0;
+ static int setHeight = 0, setLat = 0, setLong = 0;
+ struct frontend_pid_data pid_data;
+ static int mask;
+ static float pval = 0, ival = 0, dval = 0;
+ static struct option long_options[] = {
+ /* These options don’t set a flag. We distinguish them by their indices. */
+ {"roll", no_argument, &setRoll, 1},
+ {"pitch", no_argument, &setPitch, 1},
+ {"yaw", no_argument, &setYaw, 1},
+ {"rollv", no_argument, &setRollV, 1},
+ {"pitchv", no_argument, &setPitchV, 1},
+ {"yawv", no_argument, &setYawV, 1},
+ {"height", no_argument, &setHeight, 1},
+ {"lat", no_argument, &setLat, 1},
+ {"long", no_argument, &setLong, 1},
+ {0, 0, 0, 0}
+ };
+
+ while (1)
+ {
+ /* getopt_long stores the option index here. */
+ int option_index = 0;
+
+ c = getopt_long(argc, argv, "p:i:d:", long_options, &option_index);
+
+ /* Detect the end of the options. */
+ if (c == -1)
+ break;
+
+ switch(c) {
+ case 'p' :
+ pid_data.p = atof(optarg);
+ mask |= SET_P;
+ break;
+ case 'i' :
+ pid_data.i = atof(optarg);
+ mask |= SET_I;
+ break;
+ case 'd' :
+ pid_data.d = atof(optarg);
+ mask |= SET_D;
+ break;
+ default :
+ break;
+ }
+ }
+
+ if(setRoll) {
+ pid_data.controller = PID_ROLL;
+ } else if (setYaw) {
+ pid_data.controller = PID_YAW;
+ } else if (setPitch) {
+ pid_data.controller = PID_PITCH;
+ } else if (setRollV) {
+ pid_data.controller = PID_ROLL_RATE;
+ } else if (setPitchV) {
+ pid_data.controller = PID_PITCH_RATE;
+ } else if (setYawV) {
+ pid_data.controller = PID_YAW_RATE;
+ } else if (setHeight) {
+ pid_data.controller = PID_HEIGHT;
+ } else if (setLong) {
+ pid_data.controller = PID_LAT;
+ } else if (setLat) {
+ pid_data.controller = PID_LONG;
+ }
+
+ frontend_setpid(conn, &pid_data, mask);
+ return 0;
+}
diff --git a/groundStation/src/cli/cli_setpid.h b/groundStation/src/cli/cli_setpid.h
new file mode 100644
index 0000000000000000000000000000000000000000..87fb0a4f40e91ed704972bd17e4859991d91641b
--- /dev/null
+++ b/groundStation/src/cli/cli_setpid.h
@@ -0,0 +1,8 @@
+#ifndef _CLI_SETPID_H
+#define _CLI_SETPID_H
+
+#include "frontend_setpid.h"
+
+int cli_setpid(struct backend_conn * conn, int argc, char ** argv);
+
+#endif
diff --git a/groundStation/src/frontend/frontend_common.c b/groundStation/src/frontend/frontend_common.c
new file mode 100644
index 0000000000000000000000000000000000000000..cbedbe42a6dcbed1622f2109d833a75a1ce6d7ba
--- /dev/null
+++ b/groundStation/src/frontend/frontend_common.c
@@ -0,0 +1,93 @@
+#define _GNU_SOURCE
+
+#include "frontend_common.h"
+#include "config.h"
+
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <unistd.h>
+#include <err.h>
+
+struct backend_conn {
+ FILE * socket;
+
+ size_t len;
+ char * buf;
+};
+
+struct backend_conn * ucart_backendConnect()
+{
+ int s;
+ struct sockaddr_un remote;
+ char str[100];
+
+ if ((s = socket(AF_UNIX, SOCK_STREAM, 0)) == -1) {
+ perror("socket");
+ exit(1);
+ }
+
+ struct backend_conn * conn = NULL;
+
+ printf("Trying to connect...\n");
+
+ remote.sun_family = AF_UNIX;
+ char * sock_env = getenv(SOCKET_ENV);
+ strcpy(remote.sun_path, sock_env ? sock_env : DEFAULT_SOCKET);
+ if (connect(s, (struct sockaddr *)&remote, sizeof(remote)) == -1) {
+ perror("connect");
+ goto fail_final;
+ }
+
+ conn = malloc(sizeof(struct backend_conn));
+ if (conn == NULL) {
+ perror("malloc");
+ goto fail_sock;
+ }
+
+ conn->len = 0;
+ conn->buf = NULL;
+ conn->socket = fdopen(s, "r+");
+ if (conn->socket == NULL) {
+ perror("fdopen");
+ goto fail_malloc_conn;
+ }
+ if (setvbuf(conn->socket, NULL, _IONBF, 0)) {
+ warn("setvbuf");
+ }
+
+ /* success */
+ goto fail_final;
+
+fail_malloc_conn:
+ free(conn);
+ conn = NULL;
+fail_sock:
+ close(s);
+fail_final:
+ return conn;
+}
+
+void ucart_backendDisconnect(struct backend_conn * conn)
+{
+ fclose(conn->socket);
+ if (conn->buf) {
+ free(conn->buf);
+ }
+ free(conn);
+}
+
+char * ucart_backendGetline(struct backend_conn *conn)
+{
+ getline(&conn->buf, &conn->len, conn->socket);
+ return conn->buf;
+}
+
+int ucart_backendWrite(struct backend_conn *conn, const char * line)
+{
+ return fputs(line, conn->socket);
+}
diff --git a/groundStation/src/frontend/frontend_common.h b/groundStation/src/frontend/frontend_common.h
new file mode 100644
index 0000000000000000000000000000000000000000..c8ea7877190c29ff41f02f9aa92a3987c80e2de6
--- /dev/null
+++ b/groundStation/src/frontend/frontend_common.h
@@ -0,0 +1,22 @@
+#ifndef FRONTEND_COMMON_H
+#define FRONTEND_COMMON_H
+#include <stdlib.h>
+
+struct backend_conn;
+
+/* Start connection to quad */
+struct backend_conn * ucart_backendConnect(void);
+
+/* Stop connection to quad */
+void ucart_backendDisconnect(struct backend_conn * conn);
+
+/* Get a line from the backend.
+ *
+ * The line will remain valid until the next call to ucart_backendGetline.
+ */
+char * ucart_backendGetline(struct backend_conn * conn);
+
+/* Write a line to the backend */
+int ucart_backendWrite(struct backend_conn * backend, const char * line);
+
+#endif
diff --git a/groundStation/src/frontend/frontend_getpid.c b/groundStation/src/frontend/frontend_getpid.c
new file mode 100644
index 0000000000000000000000000000000000000000..fbde309c3169e4d2429f5c3cd06c7674709491a8
--- /dev/null
+++ b/groundStation/src/frontend/frontend_getpid.c
@@ -0,0 +1,46 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "frontend_getpid.h"
+#include "pid_common.h"
+
+/* Get a specified PID.
+ *
+ * Example:
+ *
+ * struct frontend_pid_data pid_data;
+ * pid_data.pid = PID_PITCH;
+ * if (frontend_getpid(conn, &pid_data)) {
+ * error
+ * } else {
+ * pid_data.p, pid_data.i, and pid_data.d are filled
+ * }
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getpid(
+ struct backend_conn * conn, struct frontend_pid_data * pid_data) {
+
+ char line[100] = "";
+ switch (pid_data->controller) {
+ case PID_PITCH :
+ strncpy(line, "getpitchp\ngetpitchd\n", 20);
+ break;
+ case PID_ROLL :
+ strncpy(line, "getrollp\ngetrolld\n", 18);
+ break;
+ case PID_YAW :
+ strncpy(line, "getyawp\ngetyawd\n", 17);
+ break;
+ default :
+ return 1;
+ }
+
+ int size;
+ if((size = ucart_backendWrite(conn, line)) < 0 ) {
+ return 1;
+ }
+
+ return 0;
+}
diff --git a/groundStation/src/frontend/frontend_getpid.h b/groundStation/src/frontend/frontend_getpid.h
new file mode 100644
index 0000000000000000000000000000000000000000..977ae97f314a3c5b3891f5d60e56929aa03d6db9
--- /dev/null
+++ b/groundStation/src/frontend/frontend_getpid.h
@@ -0,0 +1,26 @@
+#ifndef FRONTEND_GETPID_H
+#define FRONTEND_GETPID_H
+
+#include "frontend_common.h"
+#include "pid_common.h"
+
+/* Get a specified PID.
+ *
+ * Example:
+ *
+ * struct frontend_pid_data pid_data;
+ * pid_data.pid = PITCH;
+ * if (frontend_getpid(conn, &pid_data)) {
+ * error
+ * } else {
+ * pid_data.p, pid_data.i, and pid_data.d are filled
+ * }
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getpid(
+ struct backend_conn * conn,
+ struct frontend_pid_data * pid_data);
+
+
+#endif
diff --git a/groundStation/src/frontend/frontend_setpid.c b/groundStation/src/frontend/frontend_setpid.c
new file mode 100644
index 0000000000000000000000000000000000000000..58bf4109cd1c50d22486e464c101c9f8dd3c367e
--- /dev/null
+++ b/groundStation/src/frontend/frontend_setpid.c
@@ -0,0 +1,82 @@
+#include <err.h>
+#include <stdio.h>
+
+#include "frontend_setpid.h"
+#include "pid_common.h"
+#include "frontend_common.h"
+
+int frontend_setpid(
+ struct backend_conn * conn,
+ struct frontend_pid_data * pid_data,
+ int mask)
+{
+ if (conn == NULL) {
+ return -1;
+ }
+
+ char * controller;
+ switch (pid_data->controller) {
+ case PID_PITCH:
+ controller = "pitch";
+ break;
+ case PID_ROLL:
+ controller = "roll";
+ break;
+ case PID_YAW:
+ controller = "yaw";
+ break;
+ case PID_PITCH_RATE:
+ controller = "pitchrate";
+ break;
+ case PID_ROLL_RATE:
+ controller = "rollrate";
+ break;
+ case PID_YAW_RATE:
+ controller = "yawrate";
+ break;
+ case PID_HEIGHT:
+ controller = "height";
+ break;
+ case PID_LAT:
+ controller = "lat";
+ break;
+ case PID_LONG:
+ controller = "long";
+ break;
+ default:
+ warnx("Unsupported PID constant");
+ return -1;
+ }
+
+ char buffer[2048];
+ /* Set the P, I, and D */
+ if (mask & SET_P) {
+ if (snprintf(buffer, 2048,
+ "set%sp %f\n",
+ controller,
+ pid_data->p) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+ if (mask & SET_I) {
+ if (snprintf(buffer, 2048,
+ "set%si %f\n",
+ controller,
+ pid_data->i) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+ if (mask & SET_D) {
+ if (snprintf(buffer, 2048,
+ "set%sd %f\n",
+ controller,
+ pid_data->d) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+
+ return 0;
+}
diff --git a/groundStation/src/frontend/frontend_setpid.h b/groundStation/src/frontend/frontend_setpid.h
new file mode 100644
index 0000000000000000000000000000000000000000..f1deb5d3d91568939d5bd44ffab07f082a290233
--- /dev/null
+++ b/groundStation/src/frontend/frontend_setpid.h
@@ -0,0 +1,17 @@
+#ifndef _FRONTEND_SETPID_H
+#define _FRONTEND_SETPID_H
+
+#include "pid_common.h"
+#include "frontend_common.h"
+
+int frontend_setpid(
+ struct backend_conn * conn,
+ struct frontend_pid_data * pid_data,
+ int mask);
+
+#define SET_P 0x01
+#define SET_I 0x02
+#define SET_D 0x04
+#define SET_ALL (SET_P | SET_I | SET_D)
+
+#endif
diff --git a/groundStation/src/frontend/frontend_tracker.c b/groundStation/src/frontend/frontend_tracker.c
new file mode 100644
index 0000000000000000000000000000000000000000..94597b63c66d96c4d5197dd94edf138b38a956a2
--- /dev/null
+++ b/groundStation/src/frontend/frontend_tracker.c
@@ -0,0 +1,42 @@
+#include <err.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "frontend_tracker.h"
+
+#define MAGIC "TRACKERDATA"
+
+int frontend_track(
+ struct backend_conn * conn,
+ struct frontend_tracker_data * data)
+{
+ ucart_backendWrite(conn, MAGIC "\n");
+
+ char * line;
+ for (;;) {
+ line = ucart_backendGetline(conn);
+ if (line == NULL) {
+ warnx("Line not returned from backend");
+ return 1;
+ }
+
+ if (strncmp(line, MAGIC, strlen(MAGIC)) == 0) {
+ break;
+ }
+ }
+
+ if (strncmp(line, MAGIC " ERROR", strlen(MAGIC " ERROR")) == 0) {
+ warnx("Backend returned an error: %s", strstr(line, "ERROR"));
+ return 1;
+ }
+
+ /* Line format: Height Lat Long Pitch Roll Yaw */
+ sscanf(line, MAGIC " %lf %lf %lf %lf %lf %lf ",
+ &data->height,
+ &data->lateral,
+ &data->longitudinal,
+ &data->pitch,
+ &data->roll,
+ &data->yaw);
+ return 0;
+}
diff --git a/groundStation/src/frontend/frontend_tracker.h b/groundStation/src/frontend/frontend_tracker.h
new file mode 100644
index 0000000000000000000000000000000000000000..5fa82487cafb8650a4f990ef9fed0c4ea4154405
--- /dev/null
+++ b/groundStation/src/frontend/frontend_tracker.h
@@ -0,0 +1,27 @@
+#ifndef _FRONTEND_TRACKER_H
+#define _FRONTEND_TRACKER_H
+
+#include "frontend_common.h"
+
+/* Struct containing pos/att data */
+struct frontend_tracker_data {
+ double height;
+ double lateral;
+ double longitudinal;
+ double pitch;
+ double roll;
+ double yaw;
+};
+
+/* Get pos/att data from the tracking system
+ *
+ * conn: IN Connection to quad
+ * data: OUT Data is written to this struct
+ *
+ * Returns 0 on success, nonzero on error
+ *
+ */
+int frontend_track(struct backend_conn * conn,
+ struct frontend_tracker_data *data);
+
+#endif
diff --git a/groundStation/src/frontend/pid_common.h b/groundStation/src/frontend/pid_common.h
new file mode 100644
index 0000000000000000000000000000000000000000..c40c4fb42fb19bf7caf22a03effe091fb7621ece
--- /dev/null
+++ b/groundStation/src/frontend/pid_common.h
@@ -0,0 +1,26 @@
+#ifndef PID_COMMON_H
+#define PID_COMMON_H
+
+enum pid_controller {
+ PID_PITCH,
+ PID_ROLL,
+ PID_YAW,
+ PID_PITCH_RATE,
+ PID_ROLL_RATE,
+ PID_YAW_RATE,
+ PID_HEIGHT, /* Z */
+ PID_LAT, /* X */
+ PID_LONG, /* Y */
+ PID_NUM_PIDS
+};
+
+struct frontend_pid_data {
+ enum pid_controller controller;
+
+ float p;
+ float i;
+ float d;
+};
+
+
+#endif
diff --git a/groundStation/src/vrpn b/groundStation/src/vrpn
new file mode 160000
index 0000000000000000000000000000000000000000..99c54dbefe04897cc7c146101a126f62c0e65f97
--- /dev/null
+++ b/groundStation/src/vrpn
@@ -0,0 +1 @@
+Subproject commit 99c54dbefe04897cc7c146101a126f62c0e65f97