Merge branch 'master' of https://git.ece.iastate.edu/danc/MicroCART_17-18

groundStation/.gitignore

@@ -40,3 +40,5 @@ src/vrpn/pc_linux64/*
 #Exacutables
 BlueTooth
 logs
+client
+BackEnd

groundStation/Makefile

@@ -2,7 +2,7 @@
 GCC=gcc
 GXX=g++
 CFLAGS= -Wall -Wpedantic -Wextra -Werror -std=c99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
-CXXFLAGS= -Wall -Wno-reorder -std=c++11 -g
+CXXFLAGS= -Wall -Wpedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
 INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
 
 # Directories
@@ -11,7 +11,7 @@ INCDIR=inc src src/vrpn src/vrpn/quat src/vrpn/build
 OBJDIR=obj
 
 # Final exacutable name
-EXE=BlueTooth
+EXE=BackEnd
 
 # File names
 CSOURCES := $(foreach dir, $(SRCDIR), $(wildcard $(dir)/*.c )) 

groundStation/README.md

-# groundStation
+# groundStation
\ No newline at end of file
+
+## 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 ./BackEnd
+
+If you wish to change the way the backend communicates to the quad and vice versa, modify src/config.h .

groundStation/src/config.h

@@ -7,4 +7,13 @@
 #define NOQUAD_ENV "UCART_NO_QUAD"
 
 
+// If you are planning on using any of these env vars and you have
+// 	exported them with normal user rights. You will need to run the 
+// 	backend with sudo elevation and with the --preserve-env flag or -E
+#define QUAD_WIFI_ENV "UCART_USE_WIFI"
+#define QUAD_IP_ENV "UCART_QUAD_IP"
+#define QUAD_IP_DEFAULT "192.168.4.1"
+#define QUAD_PORT_ENV "UCART_QUAD_PORT"
+#define QUAD_PORT_DEFAULT 8080
+
 #endif

groundStation/src/microcart_cli.c

@@ -4,6 +4,7 @@
  */
 
 #define _GNU_SOURCE
+#define _BSD_SOURCE
 
 //system includes
 #include <err.h>
@@ -13,11 +14,18 @@
 #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"
@@ -30,16 +38,15 @@
 #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
 
 // function prototypes
-void killHandler(int);
 void readAndPrint(void);
 void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
 void sendStartPacket(void);
 void getVRPNPacket(struct ucart_vrpn_TrackerData *);
 void printVrpnData(struct ucart_vrpn_TrackerData *);
 int connectToZybo();
-int startsWith(const char *, const char *);
 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);
@@ -60,6 +67,14 @@ 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;
@@ -77,7 +92,7 @@ fd_set rfds_master;
 int max_fd = 0;
 
 pthread_mutex_t quadResponseMutex, cliInputMutex ;
-unsigned char *respBuf, *commandBuf;
+char *respBuf, *commandBuf;
 int newQuadResponse = 0, newCliInput = 0;
 
 // Structures to be used throughout
@@ -86,27 +101,17 @@ 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);
+		//updateLogFile(td);
 	}
-
 	count++;
-	// This will print the vrpn data to the terminal if necissary.
-	// Commented out because the callback will cover quad log data
-	//   at the end of flight.
-	/**if(!(count % 100)) {
-	 	printVrpnData(td);
-	 	printf("[Info] Received %d tracker updates.\n", count);
-	}**/
 }
 
-
 int main(int argc, char **argv)
 {
 	int activity;
@@ -153,26 +158,24 @@ int main(int argc, char **argv)
 		client_buffers[i][0] = '\n';
 	}
 
-	//signal(SIGINT, killHandler);
-
 	if (pthread_mutex_lock(&quadSocketMutex)) {
 		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
 	}
 
-	// if ((zyboSocket = connectToZybo()) < 0)
-	// {
-	// 	perror("Error connecting to Zybo...");
-	// 	free(respBuf);
-	// 	free(commandBuf);
-	// 	exit(1);
-	// }
+	if ((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__);
 	}
 	
 	// create vrpnTracker instance
-	//tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
+	tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
 
 	// open the log file
 	if(createLogFile(argc, argv[1]))
@@ -180,30 +183,26 @@ int main(int argc, char **argv)
 		perror("Error creating log file...");
 		exit(1);
 	}
-//	writeStringToLog(logHeader);
+	// writeStringToLog(logHeader);
 
 	// watch for input from stdin (fd 0) to see when it has input
 	safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
 	// watch for input from the zybo socket
-	//safe_fd_set(zyboSocket, &rfds_master, &max_fd);
-
-	//printf("zyboSocket = %d, max_fd = %d\n", zyboSocket, max_fd);
-
-	//tell the quad we are ready to send it vrpn data
-	printf("sending Start Packet...\n");
-	//sendStartPacket();
+	safe_fd_set(zyboSocket, &rfds_master, &max_fd);
 
+	// Tell the quad we are ready to send it vrpn data
+	sendStartPacket();
 
 	// this function will be called whenever tracker receives data
-	// ucart_vrpn_tracker_addCallback(tracker, cb);
-
-	// start the prompt
-	// fprintf(stdout, "$microcart> ");
+	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;
@@ -212,11 +211,11 @@ int main(int argc, char **argv)
 		if(activity == -1) {
 			perror("select() ");
 		} else if (activity) {
-			fprintf(stderr, "Select activity = %d\n", activity);
 			for(int fd = 0; fd <= max_fd; ++fd) {
 				if (FD_ISSET(fd, &rfds)) {
-					if(wasDisconnected(fd))
+					if(wasDisconnected(fd)){
 						break;
+					}
 
 					if (fd == fileno(stdin)) {
 						// break;
@@ -238,23 +237,43 @@ int main(int argc, char **argv)
 						// 	// Write the command to the control_loop socket
 						// 	// int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
 						// 	// if(n < 0) {
-						// 	// 	fprintf(stdout, "CLI: ERROR writing to socket\n");
+						// 	// 	printf("CLI: ERROR writing to socket\n");
 						// 	// }
 						// }
 
-						// fprintf(stdout, "$microcart> ");
+						// printf("$microcart> ");
 						// memset(userCommand, 0, cmdLen);
 					} else if (fd == zyboSocket) {
-						
+						// Read the response from the control loop
+						int available;
+						ioctl(fd, FIONREAD, &available);
+						if (available < 12) {
+							continue;
+						}
+						int respLen = readQuad(respBuf, 12);
+						if(respLen <= 0) {
+							perror("ERROR reading from quad...\n");
+						}
+						int id = getInt((unsigned char *)respBuf, 7);
+						findTimeDiff(id);
+						// 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 {
-							fprintf(stderr, "Connection\n");
+							printf("Connection\n");
 							if (new_client(new_fd)) {
-								fprintf(stderr, "Added client\n");
+								printf("Added client\n");
 								safe_fd_set(new_fd, &rfds_master, &max_fd);
 							}
 						}
@@ -269,25 +288,17 @@ int main(int argc, char **argv)
 		}
 	}
 
-	// ucart_vrpn_tracker_freeInstance(tracker);
+	ucart_vrpn_tracker_freeInstance(tracker);
 	
 	safe_close_fd(zyboSocket, &quadSocketMutex);
 
-	
-
 	closeLogFile();
 	return 0;
 }
 
-// signal handler to exit while loop of main function
-void killHandler(int dummy) {
-	keepRunning = 0;
-	printf("\nleaving Bluetooth module\n");
-}
-
 void sendStartPacket() {
 	unsigned char packet[8] = {0};
-
+	currMessageID++;
 	metadata_t metadata = 
 	{
 		(char) BEGIN_CHAR,
@@ -300,8 +311,8 @@ void sendStartPacket() {
 	packet[0] = metadata.begin_char; // BEGIN					//PACKET_START_BYTE;
 	packet[1] = metadata.msg_type;	// UPDATE				//'U'; // U for vrpn camera update, C for command
 	packet[2] = metadata.msg_subtype; // BEGIN UPDATE
-	packet[3] = 1; 			// MSG ID(1)
-	packet[4] =	0; // MSG ID(2)
+	packet[3] = (currMessageID & 0x000000ff); 			// MSG ID(1)
+	packet[4] =	((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
 	packet[5] = 0; 			// DATALEN(1)
 	packet[6] = 0; 	// DATALEN(2)
 	
@@ -319,19 +330,20 @@ void sendStartPacket() {
 	}else
 	{
 		printf("Start packet successfuly sent...\n");
-	}
+	}	
 }
 
 void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 	int pSize = sizeof(info) + 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] = (0x00 & 0x000000ff); 			// MSG ID(1)
-	packet[4] =	((0x00 >> 8) & 0x000000ff); // MSG ID(2)
+	packet[3] = (currMessageID & 0x000000ff); 			// MSG ID(1)
+	packet[4] =	((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
 	packet[5] = (sizeof(info) & 0x000000ff); 			// DATALEN(1)
 	packet[6] = ((sizeof(info) >> 8) & 0x00000ff); 	// DATALEN(2)
 	memcpy(&packet[7], &info, sizeof(info));
@@ -348,6 +360,9 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 		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) {
@@ -366,40 +381,78 @@ void printVrpnData(struct ucart_vrpn_TrackerData * td) {
 
 int connectToZybo() {
 	int sock;
-	struct sockaddr_rc addr;
+	int status = 0;
 
-	// allocate a socket	
-	sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+	/* Use bluetooth by default */
+	if (!getenv(QUAD_WIFI_ENV)) {
+		printf("Using BT Settings\n");
+		struct sockaddr_rc addr;
 
-	//set the connection params ie. who to connect to	
-	addr.rc_family = AF_BLUETOOTH;
-	addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
-	str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
-	
-	printf("Attempting to connect to zybo. Please be patient...\n");
-	// blocking call to connect to socket sock ie. zybo board
-	int status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+		// 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);
-		printf("Connection failed!...\n");
+		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;
 	}
 }
 
-int startsWith(const char *pre, const char *str) {
-    size_t lenpre = strlen(pre),
-           lenstr = strlen(str);
-    return lenstr < lenpre ? 0 : (strncmp(pre, str, lenpre) == 0);
-}
-
 /* add a fd to fd_set, and update max_fd */
 int safe_fd_set(int fd, fd_set* fds, int* max_fd) {
     assert(max_fd != NULL);
@@ -424,13 +477,14 @@ int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
 
 static ssize_t writeQuad(const char * buf, size_t count) {
 	ssize_t retval;
-	if (1) {//getenv(NOQUAD_ENV)) {
+	if (getenv(NOQUAD_ENV)) {
 		return count;
 	}
 	if (pthread_mutex_lock(&quadSocketMutex)) {
 		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
 	}
-	retval = writeQuad(buf, count);
+	//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__);
 	}
@@ -438,8 +492,22 @@ static ssize_t writeQuad(const char * buf, size_t count) {
 	return retval;
 }
 
-static int new_client(int fd)
-{
+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) {
@@ -458,8 +526,7 @@ static int new_client(int fd)
 	return 1;
 }
 
-static ssize_t get_client_index(int fd)
-{
+static ssize_t get_client_index(int fd) {
 	for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
 		if (client_fds[i] == fd) {
 			return i;
@@ -469,8 +536,7 @@ static ssize_t get_client_index(int fd)
 	return -1;
 }
 
-static char * get_client_buffer(int fd)
-{
+static char * get_client_buffer(int fd) {
 	ssize_t slot = get_client_index(fd);
 	if (slot == -1) {
 		return NULL;
@@ -501,8 +567,7 @@ static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
 	}
 }
 
-static void client_recv(int fd)
-{
+static void client_recv(int fd) {
 	char * buffer;
 	ssize_t len_pre;
      	buffer = get_client_buffer(fd);
@@ -537,11 +602,11 @@ static void client_recv(int fd)
 		buffer[newline] = '\0';
 
 		unsigned char * packet;
-		// fprintf(stderr, "newline =%li, Client sent: '%s'\n", newline, buffer);
+		// printf("newline =%li, Client sent: '%s'\n", newline, buffer);
 		if(formatCommand(buffer, &packet) != -1) {
-			fprintf(stdout, "Backend sees as: %f\n", getFloat(packet, 7));
+			printf("Backend sees as: %f\n", getFloat(packet, 7));
 		} else {
-			fprintf(stderr, "Could not recognize command '%s'\n", buffer);
+			printf("Could not recognize command '%s'\n", buffer);
 		}
 		writeQuad((char *) packet, len);
 		//free(packet);
@@ -559,8 +624,38 @@ static int wasDisconnected(int fd) {
 	{
 		remove_client(fd);
 		safe_fd_clr(fd, &rfds_master, &max_fd);
-		fprintf(stderr, "fd %d has disconnect and was removed\n", 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);
+}

groundStation/src/vrpn_tracker.cpp

 #include <iostream>
 #include <algorithm>
+#include <functional>
 
 #include "vrpn_Tracker.h"
 #include "quat.h"
@@ -9,28 +10,25 @@
 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) : Tracker(server.c_str())
+	Tracker::Tracker(std::string server) : 
+		remote(server.c_str()),
+		stop_flag(0),
+		trackerData()
 	{
 	}
 
 	Tracker::Tracker(const char * server) :
 		remote(server),
 		stop_flag(0),
-		trackerData({
-			.x = 0.0,
-			.y = 0.0,
-			.z = 0.0,
-			.pitch = 0.0,
-			.roll = 0.0,
-			.yaw = 0.0,
-			.fps = 0.0,
-			.timestamp = {
-				.tv_sec = 0,
-				.tv_usec = 0
-			}
-		})
-
+		trackerData()
 	{
 		remote.register_change_handler(this, vrpn_cb);
 
@@ -92,10 +90,9 @@ namespace microcart
 		trackerData.fps = 1.0 / elapsed_time_usec;
 
 		auto td = trackerData;
-		std::for_each(cb_vector.begin(), cb_vector.end(),
-			[td](std::function<void(const TrackerData &)> &fn){
-				fn(td);
-		});
+		for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
+			(*i)(td);
+		}
 	}
 
 	void Tracker::addCallback(std::function<void(const TrackerData&)> cb) 
@@ -115,6 +112,20 @@ 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(
@@ -123,6 +134,7 @@ extern "C"
 		try {
 			auto inst = new struct ucart_vrpn_tracker;
 			inst->t = new microcart::Tracker(server);
+			return inst;
 		} catch(...) {
 			return NULL;
 		}
@@ -138,16 +150,8 @@ extern "C"
 			void (*cb)(struct ucart_vrpn_TrackerData *))
 	{
 		try {
-			inst->t->addCallback([cb](const microcart::TrackerData & td) {
-						struct ucart_vrpn_TrackerData data;
-						data.x = td.x;
-						data.y = td.y;
-						data.z = td.z;
-						data.pitch = td.pitch;
-						data.roll = td.roll;
-						data.yaw = td.yaw;
-						(*cb)(&data);
-					});
+			auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
+			inst->t->addCallback(new_cb);
 		} catch(...) {
 			return -1;
 		}

groundStation/src/vrpn_tracker.hpp

@@ -54,6 +54,7 @@ extern "C"
 namespace microcart
 {
 	struct TrackerData {
+	public:
 		double x;
 		double y;
 		double z;
@@ -64,6 +65,8 @@ namespace microcart
 
 		double fps;
 		timeval timestamp;
+
+		TrackerData();
 	};
 
 	class Tracker {

wobsite/.gitignore

@@ -112,3 +112,6 @@ crashlytics-build.properties
 # Bundler
 .bundle
 vendor
+
+Gemfile.lock
+PASSWORD
\ No newline at end of file

wobsite/Gemfile.lock

-GEM
-  remote: https://rubygems.org/
-  specs:
-    addressable (2.4.0)
-    colorator (1.1.0)
-    ffi (1.9.14)
-    forwardable-extended (2.6.0)
-    jekyll (3.3.0)
-      addressable (~> 2.4)
-      colorator (~> 1.0)
-      jekyll-sass-converter (~> 1.0)
-      jekyll-watch (~> 1.1)
-      kramdown (~> 1.3)
-      liquid (~> 3.0)
-      mercenary (~> 0.3.3)
-      pathutil (~> 0.9)
-      rouge (~> 1.7)
-      safe_yaml (~> 1.0)
-    jekyll-sass-converter (1.4.0)
-      sass (~> 3.4)
-    jekyll-watch (1.5.0)
-      listen (~> 3.0, < 3.1)
-    kramdown (1.12.0)
-    liquid (3.0.6)
-    listen (3.0.8)
-      rb-fsevent (~> 0.9, >= 0.9.4)
-      rb-inotify (~> 0.9, >= 0.9.7)
-    mercenary (0.3.6)
-    pathutil (0.14.0)
-      forwardable-extended (~> 2.6)
-    rb-fsevent (0.9.7)
-    rb-inotify (0.9.7)
-      ffi (>= 0.5.0)
-    rouge (1.11.1)
-    safe_yaml (1.0.4)
-    sass (3.4.22)
-
-PLATFORMS
-  ruby
-
-DEPENDENCIES
-  jekyll
-
-BUNDLED WITH
-   1.10.6

wobsite/Makefile

+.PHONY : build deploy
+
+build:
+	jekyll build
+	chmod -R 744 _site/
+
+deploy:
+	make build
+	./deploy
\ No newline at end of file

wobsite/README.md

@@ -25,3 +25,26 @@ See [docs](https://github.com/wemake-services/jekyll-theme-hackcss/blob/master/d
 ## License
 
 GNU PGL 3. See [LICENSE](https://github.com/wemake-services/jekyll-theme-hackcss/blob/3cbe97b71a56a19eba386dd928e125b71e50c71e/LICENSE) for more information.
+
+## Contributing
+
+Create or edit a page in this directory. Use the `.md` extension.
+
+When you are ready to build the site:
+
+```
+make build
+```
+
+If you want to run the site on localhost:
+
+```
+bundle exec jekyll serve
+# http://localhost:4000
+```
+
+To deploy the website to our team url:
+
+```
+make deploy
+```
\ No newline at end of file

wobsite/_config.yml

@@ -30,15 +30,18 @@ email: microcart_f2016@iastate.edu
 navigation:
   - text: Home
     url: /
-  - text: Test Page
-    url: /test.html
   - text: Team Members
     url: /members.html
+  - text: Quadcopter
+    url: /quadcopter.html
   - text: Ground Station
     url: /ground.html
+  - text: Controls
+    url: /controls.html
   - text: Jekyll Theme Docs
     url: /docs.html
 
+
 projects:
 
 social:
@@ -46,4 +49,9 @@ social:
 exclude:
     - Gemfile
     - Gemfile.lock
-    - vendor
+    - vendor
\ No newline at end of file
+    - Makefile
+    - README.md
+    - LICENSE
+    - deploy
+    - PASSWORD
\ No newline at end of file

wobsite/controls.md

+---
+layout: default
+---
+# Controls
+
+The quadcopter is stabilized with on-board PID controllers. These controllers
+use pitch, roll, and yaw data from on-board sensors and positional data from the
+infrared tracking system to determine appropriate actuator signals necessary
+to keep the quadcopter stable during flight.
+
+The classical PID controller is characterized by 3 coefficients that specify
+how the controller functions. A PID controller can be configured through
+iterative determination of the PID coefficients, but we expect to obtain a more
+accurate control system by first developing a robust mathematical model of the
+quadcopter system. Once we establish this model, we can use numerical
+characteristics of the quadcopter to calculate the appropriate PID coefficients
+for our controllers.
+
+The model we are developing follows the methods developed by our advising
+graduate student Matt Rich in his thesis [Model development, system
+identification, and control of a quadrotor helicopter][1].
+
+<img src="images/pid_diagram.png" width="700">
+
+[1]: http://lib.dr.iastate.edu/etd/12770/
\ No newline at end of file

wobsite/deploy

+#!/bin/bash
+
+HOST=may1716.sd.ece.iastate.edu
+USERNAME=may1716
+[ -e PASSWORD ] || echo "ERROR: File PASSWORD is missing.
+Create a file called PASSWORD, and put the sftp password in that file.
+No spaces. No newlines."
+[ -e PASSWORD ] || exit 1
+PASSWORD=$(cat PASSWORD)
+
+cd _site || exit 1
+
+lftp<<END_SCRIPT
+open sftp://$HOST
+user $USERNAME $PASSWORD
+cd www
+mirror -R
+exit
+END_SCRIPT
+
+echo "Website updated."
\ No newline at end of file

wobsite/index.md

 ---
 layout: default
 ---
-# Microcart
+# MicroCART
+## Microcontroller Controlled Aerial Robot Team
 
-Some info here.
+**Senior Design Group**: may1716
 
-## Overview and introduction
+MicroCART is an ongoing senior design project focused on the
+development of a quadcopter as a research platform for controls and
+embedded systems. This year, our team is responsible for advancing the
+modular structure the platform, developing a controls model, and
+improving the autonomous flight capabilities of the quadcopter.
 
-Blah blah blah
+For additional information regarding the project, please visit the
+[suppliemental wiki
+page][1].
 
-## Navigation to the left
-
-Click on links for detailed information about stuff.
+[1]: https://wikis.ece.iastate.edu/microcart-senior-design/index.php/2016-2017_Main_Page

wobsite/members.md

+---
+layout: default
+---
+# Members
+
+## David Wehr
+**Team Leader**  
+Computer Engineer  
+dawehr@iastate.edu  
+
+## Tara Mina
+**Communications Leader**  
+Electrical Engineer  
+tymina@iastate.edu  
+
+## Jake Drahos
+**Webmaster**  
+Computer Engineer  
+drahos@iastate.edu 
+
+## Eric Middleton
+**Hardward Maintainer**  
+Electrical/Computer Engineer  
+ericm@iastate.edu  
+
+## Joe Bush
+**Quadcopter Software Key Concept Holder**  
+Computer Engineer  
+jpbush@iastate.edu 
+
+## Kris Burney
+**Ground Station Key Concept Holder**  
+Computer Engineer  
+burneykb@iastate.edu  
+
+## Andy Snawerdt
+**Controls Systems Key Concept Holder**  
+Electrical Engineer  
+snawerdt@iastate.edu  
+
+## Brendan Bartels
+**Controls Software Key Concept Holder**  
+Electrical Engineer  
+bbartels@iastate.edu  
+
+# Advisors
+
+## Dr. Phillip Jones
+## Dr. Nicola Elia