groundStation/src/config.h

+#ifndef __BACKEND_H
+#define __BACKEND_H
+
+
+#define DEFAULT_SOCKET "/var/run/ucart.socket"
+#define SOCKET_ENV "UCART_SOCKET"
+#define NOQUAD_ENV "UCART_NO_QUAD"
+
+
+#endif

src/logger.c → groundStation/src/logger.c

@@ -4,21 +4,30 @@
  */
 #include "logger.h"
 #include <stdio.h>
+#include <err.h>
+#include <pthread.h>
 
-int quadlog_file;
+static FILE * quadlog_file = NULL;
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 
 int createLogFile(int argc, char* argv)
 {
-	char log_file[300] = {'l', 'o', 'g','s', '/'};
+	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)
 	{
-		strcat(log_file, argv);
+		strncat(log_file, argv, 294);
 		printf("Creating log file '%s'...\n",log_file);
-		quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666);
-		return quadlog_file;
-	}
-	else
-	{
+		quadlog_file = fopen(log_file, "a");
+	} else {
 		time_t rawtime;
 		char timestr [30];
 		time(&rawtime);
@@ -26,7 +35,7 @@ int createLogFile(int argc, char* argv)
 
 		// Lets convert space to _ in
 		char *p = timestr;
-		int i = 0;
+		size_t i = 0;
 		while(i < strlen(timestr))
 		{ 
 		    if (*p == ' ')
@@ -40,18 +49,60 @@ int createLogFile(int argc, char* argv)
 		strncat(log_file, timestr, strlen(timestr) -1 );
 		strcat(log_file, ".txt");
 		printf("Creating log file '%s'...\n",log_file);
-		quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666);
-		return quadlog_file;
+		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)
 {
-	return dprintf(quadlog_file, "FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
+	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)
 {
-	return dprintf(quadlog_file, "%s", 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__);
+	}
+
 }

src/logger.h → groundStation/src/logger.h

@@ -15,6 +15,7 @@
 int createLogFile(int, char*);
 int writeStringToLog(const char*);
 int updateLogFile(const struct ucart_vrpn_TrackerData* );
+void closeLogFile();
 
 
-#endif
\ No newline at end of file
+#endif

src/microcart_cli.c → groundStation/src/microcart_cli.c

@@ -2,15 +2,22 @@
  *
  * BlueTooth socket program for passing vrpn data to quad.
  */
+
+#define _GNU_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 <sys/select.h>
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/rfcomm.h>
 #include <pthread.h>
+#include <assert.h>
 
 //user created includes
 #include "communication.h"
@@ -18,6 +25,7 @@
 #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
@@ -31,213 +39,253 @@ void sendStartPacket(void);
 void getVRPNPacket(struct ucart_vrpn_TrackerData *);
 void printVrpnData(struct ucart_vrpn_TrackerData *);
 int connectToZybo();
-void *handleQuadResponse();
-void *handleCliInput();
-int atomic_check(int*, pthread_mutex_t*);
-void performCommand(char *cmdName, char * command);
-int startsWith(const char *pre, const char *str);
-
-//static void cb(struct ucart_vrpn_TrackerData *);
+// void *handleQuadResponse();
+// void *handleCliInput();
+// int atomic_check(int*, pthread_mutex_t*);
+void performCommand(char *, char *);
+int startsWith(const char *, const char *);
+int safe_fd_set(int , fd_set* , int* );
+int safe_fd_clr(int , fd_set* , int* );
+static void 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);
+/* Returns -1 on error */
+static int remove_client(int fd);
+/* Receive data from client */
+static void client_recv(int fd);
+
+/* Thread-safe wrappers */
+pthread_mutex_t quadSocketMutex;
+static ssize_t writeQuad(const char * buf, size_t count);
 
 // global variables
 static volatile int keepRunning = 1;
 const char *TRACKER_IP = "UAV@192.168.0.120:3883";
-int quadlog_file;
-int zyboSocket, status, bytes_read;
+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
+static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE];
+static int client_fds[MAX_CLIENTS];
+
 pthread_mutex_t quadResponseMutex, cliInputMutex ;
 unsigned char *respBuf, *commandBuf;
 int newQuadResponse = 0, newCliInput = 0;
 
 // Structures to be used throughout
-modular_structs_t structs = {};
+modular_structs_t structs;
 
 // Callback to be ran whenever the tracker receives data.
 // Currently doing much more than it should. It will be slimmed down 
 // 		in the future.
 
-// static void cb(struct ucart_vrpn_TrackerData * td)
-// {
-// 	static int count = 0;
-
-// 	if(!(count % 10)) {
-// 		sendVrpnPacket(td);
-// 		updateLogFile(td);
-// 	}
-
-// 	count++;
-
-// 	// This will print the vrpn data to the terminal if necissary.
-// 	// Commented out because the callback will cover quad log data
-// 	//   at the end of flight.
-// 	/**if(!(count % 100)) {
-// 	 	printVrpnData(td);
-// 	 	printf("[Info] Received %d tracker updates.\n", count);
-// 	}**/
-// }
-
-
-void *handleQuadResponse() {
-	unsigned char buffer[255];
-
-	while(keepRunning) {
-		// Clear the buffer and read the message from the socket (from the server)
-		memset(buffer, 0, 255);
-
-		// If there was an error reading from the socket, throw an error
-		if(read(zyboSocket, buffer, 255) <= 0) {
-			fprintf(stderr, "CLI QUAD: ERROR reading from quad.\n");
-			continue;
-		}
-
-		pthread_mutex_lock(&quadResponseMutex);
-		newQuadResponse = 1;
-		memcpy(respBuf, buffer, 255);
-		pthread_mutex_unlock(&quadResponseMutex);
+static void cb(struct ucart_vrpn_TrackerData * td)
+{
+	static int count = 0;
 
-		//parse_packet(buffer, &respBuf, &metadata);
-		// fprintf(stderr, "CLI QUAD: %s\n", buffer);
+	if(!(count % 10)) {
+		sendVrpnPacket(td);
+		updateLogFile(td);
 	}
 
-	pthread_exit(NULL);
+	count++;
+	// This will print the vrpn data to the terminal if necissary.
+	// Commented out because the callback will cover quad log data
+	//   at the end of flight.
+	/**if(!(count % 100)) {
+	 	printVrpnData(td);
+	 	printf("[Info] Received %d tracker updates.\n", count);
+	}**/
 }
 
-void *handleCliInput() {
-	sleep(1);
-	while(keepRunning)
-	{
-		char userCommand[CMD_MAX_LENGTH] = {};
-		fprintf(stderr, "$microcart> ");
 
-		while(fgets(userCommand, sizeof(userCommand), stdin) != NULL && keepRunning) {
-			
-			// if the user simply hit enter then let them try again
-			if((userCommand[0] == '\n') || (userCommand[0] == '\r'))
-			{
-				fprintf(stderr, "$microcart> ");
-
-				memset(userCommand, 0, CMD_MAX_LENGTH);
+int main(int argc, char **argv)
+{
+	// pthread_t quadResponse, cliInput;
+	fd_set rfds_master;
+	int activity;
+	int max_fd = 0;
+	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);
 
-				continue;
-			}
+	/* Create socket */
+	backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
+	if (backendSocket < 0) {
+		err(-1, "socket");
+	}
 
-			if((userCommand[strlen(userCommand) - 1] == '\n') || (userCommand[strlen(userCommand) - 1] == '\r'))
-			userCommand[strlen(userCommand) - 1] = '\0';
+	/* 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");
+	}
 
-			pthread_mutex_lock(&cliInputMutex);
-			newCliInput = 1;
-			memcpy(commandBuf, &userCommand, CMD_MAX_LENGTH);
-			pthread_mutex_unlock(&cliInputMutex);
+	/* Listen */
+	if (listen(backendSocket, 16)) {
+		err(-1, "listen");
+	}
 
-			fprintf(stderr, "$microcart> ");
+	/* Add to socket set */
+	safe_fd_set(backendSocket, &rfds_master, &max_fd);
+	
 
-			memset(userCommand, 0, CMD_MAX_LENGTH);
-		}
+	/* Initialize client buffers */
+	for (int i = 0; i < MAX_CLIENTS; i++) {
+		client_fds[i] = -1;
+		client_buffers[i][0] = '\n';
 	}
 
-	pthread_exit(NULL);
-}
-
-int main(int argc, char **argv)
-{
-	pthread_t quadResponse, cliInput;
-	respBuf = malloc(1024);
-	commandBuf = malloc(CMD_MAX_LENGTH);
-	
 	signal(SIGINT, killHandler);
 
-	if ((zyboSocket = connectToZybo()) < 0)
-	{
-		perror("Error connecting to Zybo...");
-		free(respBuf);
-		free(commandBuf);
-		exit(1);
+	if (pthread_mutex_lock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
 	}
-	
-	// create vrpnTracker instance
-//	tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
 
-	// Retrieve VRPN data from the control loop
-	fprintf(stderr, "CLI: Starting quad receiving thread...\n");
-	pthread_create(&quadResponse, NULL, handleQuadResponse, NULL);
-	fprintf(stderr, "CLI: Thread quad receiving started.\n");
+	// if ((zyboSocket = connectToZybo()) < 0)
+	// {
+	// 	perror("Error connecting to Zybo...");
+	// 	free(respBuf);
+	// 	free(commandBuf);
+	// 	exit(1);
+	// }
 
-	// Retrieve user command input
-	fprintf(stderr, "CLI: Starting cli input thread...\n");
-	pthread_create(&cliInput, NULL, handleCliInput, NULL);
-	fprintf(stderr, "CLI: Thread Cli input started.\n");
+	if (pthread_mutex_unlock(&quadSocketMutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+	
+	// create vrpnTracker instance
+	//tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
 
 	// open the log file
-	if( (status = createLogFile(argc, argv[1])) < 0)
+	if(createLogFile(argc, argv[1]))
 	{
 		perror("Error creating log file...");
-		free(respBuf);
-		free(commandBuf);
 		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();
+	//sendStartPacket();
+
 
 	// this function will be called whenever tracker receives data
 	// ucart_vrpn_tracker_addCallback(tracker, cb);
-	int updatePrompt = 0;
-	while(keepRunning)
-	{
-		char tmpRespBuf[1024];
-		unsigned char tmpCommandBuf[CMD_MAX_LENGTH];
 
+	// start the prompt
+	fprintf(stdout, "$microcart> ");
 
-		if(updatePrompt)
-		{
-			fprintf(stderr, "$microcart> ");
-			updatePrompt = 0;	
+	struct timeval timeout = {
+		.tv_sec = 1,
+		.tv_usec = 0
+	};
+	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) {
+			fprintf(stderr, "Select activity = %d\n", activity);
+			for(int fd = 0; fd <= max_fd; ++fd) {
+				if (FD_ISSET(fd, &rfds)) {
+					fprintf(stderr, "Select woke for fd %d\n", fd);
+					if (fd == fileno(stdin)) {
+						unsigned char userCommand[CMD_MAX_LENGTH];
+						read(fileno(stdin), (char *)userCommand, sizeof(userCommand));
+						unsigned int cmdLen = strlen((char*) userCommand);
+						// if the user simply hit enter then let them try again
+						if((userCommand[0] != '\n') && (userCommand[0] != '\r'))
+						{
+							// remove newline and return line chars
+							if((userCommand[cmdLen - 1] == '\n') || (userCommand[cmdLen - 1] == '\r'))
+							userCommand[cmdLen - 1] = '\0';
+
+							unsigned char *packet;
+							formatCommand(userCommand, &packet);
+
+							printf("received input from cli: '%s'\n", userCommand);
+
+							fprintf(stdout, "CLI sees as: %f\n", getFloat(packet, 7));
+
+							// Write the command to the control_loop socket
+							// int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
+							// if(n < 0) {
+							// 	fprintf(stdout, "CLI: ERROR writing to socket\n");
+							// }
+						}
+						
+
+						fprintf(stdout, "$microcart> ");
+						memset(userCommand, 0, cmdLen);
+					} else if (fd == zyboSocket) {
+						
+					} 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");
+							if (new_client(new_fd)) {
+								fprintf(stderr, "Added client\n");
+								safe_fd_set(new_fd, &rfds_master, &max_fd);
+							}
+						}
+					} else if (get_client_index(fd) > -1) {
+						client_recv(fd);
+					}
+				}
+			}
+		} else {
+			for(int i = 0; i < MAX_CLIENTS; ++i) {
+				if(client_fds[i] != -1) {
+					char buff;
+					if(recv(client_fds[i],&buff, 1, MSG_PEEK | MSG_DONTWAIT) < 0)
+						remove_client(client_fds[i]);
+				}
+			}
+			timeout.tv_sec = 1;
+			timeout.tv_usec = 0;
 		}
+	}
 
-		//check for user input via cli
-		if(atomic_check(&newCliInput, &cliInputMutex))
-		{
-			pthread_mutex_lock(&cliInputMutex);
-			newCliInput = !newCliInput;
-			memcpy(tmpCommandBuf, commandBuf, CMD_MAX_LENGTH);
-			pthread_mutex_unlock(&cliInputMutex);
-
-			// I can use printf becuase the command was gathered using fgets.
-			//fprintf(stderr, "\rINPUT FOUND via CLI: '%s'\n", tmpCommandBuf);
-			updatePrompt = !updatePrompt;
-			unsigned char *packet;
-			formatCommand(tmpCommandBuf, &packet);
-		}
+	// ucart_vrpn_tracker_freeInstance(tracker);
+	
+	safe_close_fd(zyboSocket, &quadSocketMutex);
 
-		//check for update/response from quad
-		if(atomic_check(&newQuadResponse, &quadResponseMutex))
-		{
-			pthread_mutex_lock(&quadResponseMutex);
-			newQuadResponse = !newQuadResponse;
-			memcpy(tmpRespBuf, respBuf, 1024);
-			pthread_mutex_unlock(&quadResponseMutex);
-
-			char buf[1025];
-			memcpy(buf, tmpRespBuf, 1024);
-			buf[1025] = '\0';
-			//fprintf(stderr, "\rINPUT FOUND via QUAD: '%s'\n", buf);
-			writeStringToLog(buf);
-			updatePrompt = !updatePrompt;
-		}
-	}
+	
 
-	//ucart_vrpn_tracker_freeInstance(tracker);
-	//free(vrpnData);
-	free(respBuf);
-	free(commandBuf);
-	pthread_mutex_destroy(&cliInputMutex);
-	pthread_mutex_destroy(&quadResponseMutex);
-	close(zyboSocket);
-	close(quadlog_file);
+	closeLogFile();
 	return 0;
 }
 
@@ -247,24 +295,12 @@ void killHandler(int dummy) {
 	printf("\nleaving Bluetooth module\n");
 }
 
-void readAndPrint() {
-	// read data from the server
-	// this is a blocking call.
-	//TODO: Implement a non blocking version of this.
-	bytes_read = read(zyboSocket, respBuf, sizeof(respBuf) -1);
-	if( bytes_read > 0)
-	{
-		respBuf[bytes_read] = '\0';
-		printf("%s", respBuf);
-	}
-}
-
 void sendStartPacket() {
 	unsigned char packet[8] = {0};
 
 	metadata_t metadata = 
 	{
-		BEGIN_CHAR,
+		(char) BEGIN_CHAR,
 		0x04,
 		0x01,
 		0x01,
@@ -285,7 +321,7 @@ void sendStartPacket() {
 		checksum ^= packet[i];
 
 	packet[metadata.data_len + 7] = checksum; //PACKET_END_BYTE;
-	status = write(zyboSocket, &packet, metadata.data_len + 8);
+	int status = writeQuad((char * ) packet, metadata.data_len + 8);
 	if (status != 8)
 	{
 		perror("Error sending start packet...\n");
@@ -299,7 +335,7 @@ void sendStartPacket() {
 void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 	int pSize = sizeof(info) + 8;
 	int n;
-	char packet[pSize];
+	unsigned char packet[pSize];
 	packet[0] = 0xBE; // BEGIN					//PACKET_START_BYTE;
 	packet[1] = 0x04;	// UPDATE				//'U'; // U for vrpn camera update, C for command
 	packet[2] = 0x00; // N/A
@@ -317,7 +353,7 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 
 	packet[pSize - 1] = checksum; //PACKET_END_BYTE;
 
-	n = write(zyboSocket, packet, pSize);
+	n = writeQuad((char *) packet, pSize);
 	if(n < 0) {	
 		perror("vrpnhandler: ERROR writing to socket");
 		keepRunning = 0;
@@ -340,7 +376,7 @@ void printVrpnData(struct ucart_vrpn_TrackerData * td) {
 
 int connectToZybo() {
 	int sock;
-	struct sockaddr_rc addr = { -1 };
+	struct sockaddr_rc addr;
 
 	// allocate a socket	
 	sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
@@ -352,7 +388,7 @@ int connectToZybo() {
 	
 	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));
+	int status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
 
 	// connection failed
 	if(status < 0)
@@ -368,18 +404,12 @@ int connectToZybo() {
 	}
 }
 
-int atomic_check(int* atomicFlag, pthread_mutex_t* mutex) {
-   pthread_mutex_lock(mutex);
-   int result = *atomicFlag;
-   pthread_mutex_unlock(mutex);
-   return result;
-}
-
 void performCommand(char *cmdName, char * command) {
 	for(int i = 0; i < NUM_COMMANDS; ++i)
 	{
-		if(startsWith(registeredCommands[i].commandText, command));
-			fprintf(stderr, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
+		if(startsWith(registeredCommands[i].commandText, command)) {
+			fprintf(stdout, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
+		}
 	}
 }
 
@@ -387,4 +417,153 @@ int startsWith(const char *pre, const char *str) {
     size_t lenpre = strlen(pre),
            lenstr = strlen(str);
     return lenstr < lenpre ? 0 : (strncmp(pre, str, lenpre) == 0);
-}
\ No newline at end of file
+}
+
+/* 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__);
+	}
+	retval = writeQuad(buf, count);
+	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 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';
+	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';
+
+
+	/* 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';
+		unsigned char * packet;
+		fprintf(stderr, "Client sent: %s\n", buffer);
+		formatCommand((unsigned char *) buffer, &packet);
+		writeQuad((char *) packet, 4);
+		free(packet);
+		
+		char * rest = &buffer[newline] + 1;
+		/* Delete parsed data and move the rest to the left */
+		memmove(buffer, rest, strlen(rest));
+	}
+}

vrpn @ 99c54dbe

+Subproject commit 99c54dbefe04897cc7c146101a126f62c0e65f97

src/vrpn/.clang-format

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

src/vrpn/.clang-tidy

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

src/vrpn/.gitmodules

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

src/vrpn/.travis.yml

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

src/vrpn/DashboardBuildInitialCache.cmake.in

-# Project configuration settings
-set(VRPN_USE_PHANTOM_SERVER "@VRPN_USE_PHANTOM_SERVER@" CACHE BOOL "")
-set(VRPN_INCLUDE_TIMECODE_SERVER "@VRPN_INCLUDE_TIMECODE_SERVER@" CACHE BOOL "")
-set(VRPN_INCLUDE_INTERSENSE "@VRPN_INCLUDE_INTERSENSE@" CACHE BOOL "")
-set(VRPN_USE_NATIONAL_INSTRUMENTS "@VRPN_USE_NATIONAL_INSTRUMENTS@" CACHE BOOL "")
-set(VRPN_USE_NATIONAL_INSTRUMENTS_MX "@VRPN_USE_NATIONAL_INSTRUMENTS_MX@" CACHE BOOL "")
-set(VRPN_USE_NIDAQ "@VRPN_USE_NIDAQ@" CACHE BOOL "")
-set(VRPN_USE_USDIGITAL "@VRPN_USE_USDIGITAL@" CACHE BOOL "")
-set(VRPN_USE_MICROSCRIBE "@VRPN_USE_MICROSCRIBE@" CACHE BOOL "")
-set(VRPN_INCLUDE_PHASESPACE "@VRPN_INCLUDE_PHASESPACE@" CACHE BOOL "")
-set(VRPN_USE_MOTIONNODE "@VRPN_USE_MOTIONNODE@" CACHE BOOL "")
-set(VRPN_USE_WIIUSE "@VRPN_USE_WIIUSE@" CACHE BOOL "")
-set(VRPN_USE_FREESPACE "@VRPN_USE_FREESPACE@" CACHE BOOL "")
-set(VRPN_USE_DIRECTINPUT "@VRPN_USE_DIRECTINPUT@" CACHE BOOL "")
-set(VRPN_USE_DIRECTSHOW "@VRPN_USE_DIRECTSHOW@" CACHE BOOL "")
-set(VRPN_USE_SHARED_LIBRARY "@VRPN_USE_SHARED_LIBRARY@" CACHE BOOL "")
-set(VRPN_USE_GPM_MOUSE "@VRPN_USE_GPM_MOUSE@" CACHE BOOL "")
-
-set(VRPN_GPL_SERVER "@VRPN_GPL_SERVER@" CACHE BOOL "")
-
-# Root directories to search for dependencies
-set(WIIUSE22_ROOT_DIR "@WIIUSE_ROOT_DIR@" CACHE PATH "")

src/vrpn/Format

-
-/*
- *
- *  VRPN Header Format
- *
- * +------------+------------+------------+------------+
- * |                    Total Length                   |
- * +------------+------------+------------+------------+
- * |                Timestamp (Seconds)                |
- * +------------+------------+------------+------------+
- * |              Timestamp (Microseconds)             |
- * +------------+------------+------------+------------+
- * |                       Sender                      |
- * +------------+------------+------------+------------+
- * |                        Type                       |
- * +------------+------------+------------+------------+
- * |                       padding                     |
- * +------------+------------+------------+------------+
- *
- * Total Length = Padded Payload Length + Padded Header Length.
- * Padded Header Length = 24 bytes.
- * Padded Payload Length = Payload Length + (Payload Length % 8).
- *
- * All header fields are 32-bit integers sent in network order.
- * Both header and payload are padded to end on 8-byte boundaries.
- *
- * Implementation in vrpn_Connection::marshall_message().
- *
- */
-
-/*
- *
- *  VRPN Connection setup sequence
- *
- * Server opens a TCP socket and listens() on port - default vrpn_DEFAULT_LISTEN_PORT_NO
- * Client connects() to port
- * Server and client both write a COOKIE
- * Server and client both wait (nonblocking) for a COOKIE and
- *   verify version #, etc.
- * If either peer is requesting remote logging, a log_description message
- *   is sent.
- * Server and client both open UDP sockets.
- * Server and client both send udp_description messages.
- * Server and client both send sender_description messages for all senders
- *   registered locally.
- * Server and client both send type_description messages for all types
- *   registered locally.
- *
- * Implementation in vrpn_Connection::setup_new_connection(),
- *   finish_new_connection_setup(),
- */
-
-/*
- * VRPN Cookie Format
- *
- * +------------+------------+------------+------------+
- * +    'v'     |    'r'     |    'p'     |    'n'     +
- * +------------+------------+------------+------------+
- * +    ':'     |    ' '     |    'v'     |    'e'     +
- * +------------+------------+------------+------------+
- * +    'r'     |    '.'     |    ' '     |     X      +
- * +------------+------------+------------+------------+
- * +     X      |    '.'     |     Y      |     Y      +
- * +------------+------------+------------+------------+
- * +    ' '     |    ' '     |     L      |   padding  |
- * +------------+------------+------------+------------+
- * +                      padding                      |
- * +------------+------------+------------+------------+
- *
- * XX = major version number (2 ASCII characters)
- * YY = minor version number (2 ASCII characters)
- *  L = remote log mode requested (ASCII '0', '1', '2', or '3')
- *
- * This cookie is in ASCII so that humans scanning a dump can
- * quickly determine whether or not it is a VRPN session.
- */
-
-/*
- * VRPN Log description message:
- *
- * type = vrpn_CONNECTION_LOG_DESCRIPTION (-4)
- * sender = logging mode (0, 1, 2, or 3)
- * payload length = strlen(logfile name) + 1
- * data = null-terminated ASCII string giving pathname of file to log to
- */
-
-/*
- * VRPN UDP description message:
- *
- * type = vrpn_CONNECTION_UDP_DESCRIPTION (-3)
- * sender = UDP port number to contact
- * payload length = strlen(IP address) + 1
- * data = ASCII string containing host's IP address
- */
-
-/*
- * VRPN Sender description message:
- *
- * type = vrpn_CONNECTION_SENDER_DESCRIPTION (-1)
- * sender = ID of sender
- * payload length = 5 + strlen(sender name)
- * data = 
- *
- * +------------+------------+------------+------------+
- * +             length of sender name + 1             |
- * +------------+------------+------------+------------+
- * +                    sender name                    |
- * +------------+------------+------------+------------+
- * +                        ...                        |
- * +------------+------------+------------+------------+
- *
- * N.B. There is redundant length data here
- */
-
-/*
- * VRPN Type description message:
- *
- * type = vrpn_CONNECTION_TYPE_DESCRIPTION (-2)
- * sender = ID of type
- * payload length = 5 + strlen(sender name)
- * data =
- *
- * +------------+------------+------------+------------+
- * +              length of type name + 1              |
- * +------------+------------+------------+------------+
- * +                     type name                     |
- * +------------+------------+------------+------------+
- * +                        ...                        |
- * +------------+------------+------------+------------+
- *
- * N.B. There is redundant length data here
- */
-
-/*
- * VRPN disconnect message:
- *
- * type = vrpn_CONNECTION_DISCONNECT_MESSAGE (-5)
- * sender = 0
- * payload length = 0
- *
- * This message is never sent over the wire;  it is written into logs
- * to mark the point at which a connection was broken.
- */
-