Merge branch 'groundStation-dev-comp_graph-commands' of...

Merge branch 'groundStation-dev-comp_graph-commands' of git.ece.iastate.edu:danc/MicroCART_17-18 into groundStation-dev-comp_graph-commands

groundStation/src/backend/backend.c

@@ -152,6 +152,8 @@ int main(int argc, char **argv)
 		err(-1, "socket");
 	}
 
+	printf("backendSocket = %d\n", backendSocket);
+
 	/* Create sockaddr and bind */
 	struct sockaddr_un sa;
 	sa.sun_family = AF_UNIX;
@@ -190,6 +192,8 @@ int main(int argc, char **argv)
 		exit(1);
 	}
 
+	printf("zyboSocket = %d\n", zyboSocket);
+
 	if (pthread_mutex_unlock(&quadSocketMutex)) {
 		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
 	}
@@ -218,10 +222,13 @@ int main(int argc, char **argv)
 	sendStartPacket();
 
 	if(!getenv(NOVRPN_ENV)){
+		printf("Creating VRPN tracker...\n");
 		// 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);	
+	} else {
+		printf("Ignoring VRPN information...\n");
 	}
 
 	struct timeval timeout = {
@@ -556,6 +563,7 @@ static int clientAddPendResponses(int fd, uint16_t packet_id) {
 static int clientRemovePendResponses(int fd, uint16_t packet_id) {
 	int *pendingResponses = get_client_pend_responses(fd);
 	for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
+		printf("might remove pendingResponses[%d], if it equals %d\n", i, packet_id);
 		if(pendingResponses[i] == packet_id) {
 			pendingResponses[i] = -1;
 			return i;
@@ -729,12 +737,13 @@ static void client_recv(int fd) {
 			}
 
 			/* Only add the client to the pending responses if it was a getparam command */
-			if (m.msg_type == GETPARAM_ID || m.msg_type == GETOUTPUT_ID || 
-				m.msg_type == GETSOURCE_ID || m.msg_type == GETNODES_ID || m.msg_type == ADDNODE_ID) {
-				if (clientAddPendResponses(fd, BytesTo16(packet[ID_L], packet[ID_H])) == -1) {
-					warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
-				}
-			}
+			// if (m.msg_type == GETPARAM_ID || m.msg_type == GETOUTPUT_ID || 
+			// 	m.msg_type == GETSOURCE_ID || m.msg_type == GETNODES_ID || m.msg_type == ADDNODE_ID) {
+			// 	printf("adding fd %d with id %d\n", fd, BytesTo16(packet[ID_L], packet[ID_H]));
+			// 	if (clientAddPendResponses(fd, BytesTo16(packet[ID_L], packet[ID_H])) == -1) {
+			// 		warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
+			// 	}
+			// }
 
 			printf("packetToQuad = '");
 			for(int i = 0; i < (int)psize; ++i) {
@@ -742,7 +751,8 @@ static void client_recv(int fd) {
 			}
 			printf("'\n");
 			
-			writeQuad(packet, psize);
+			int retval = writeQuad(packet, psize);
+			printf("sent %d bytes\n", retval);
 			free(data);
 		}
 
@@ -771,13 +781,13 @@ static void quad_recv() {
 		return;
 	}
 	respBufLen += respLen;
-/*
+
 	printf("packetFromQuad = '");
 	for(int i = 0; i < (int)respBufLen; ++i) {
 		printf(" %.2x ", respBuf[i]);
 	}
 	printf("'\n");
-*/
+
 	while(respBufLen){
 		datalen = DecodePacket(&m, data, CMD_MAX_LENGTH, respBuf, respBufLen);
 		if (datalen == -1) {
@@ -880,12 +890,12 @@ static void handleResponse(struct metadata *m, uint8_t * data)
 		return;
 	}
 
-	for(int fd = 0; fd <= max_fd; ++fd) {
-		if (get_client_index(fd) < 0) {
-			clientRemovePendResponses(fd, m->msg_id);
-			write(fd, buffer, result);
-		}
-	}
+	// for(int fd = 0; fd <= max_fd; ++fd) {
+	// 	if (get_client_index(fd) < 0) {
+	// 		clientRemovePendResponses(fd, m->msg_id);
+	// 		write(fd, buffer, result);
+	// 	}
+	// }
 	free(buffer);
 }
 

quad/src/virt_quad/hw_impl_unix_uart.c

@@ -3,6 +3,7 @@
 #include <sys/socket.h>
 #include <stdio.h>
 #include <sys/un.h>
+#include <sys/ioctl.h>
 #include <err.h>
 #include <netinet/in.h>
 
@@ -10,18 +11,17 @@
 #define SOCKET_ENV "VIRT_QUAD_SOCKET"
 
 static int backendSocket;
-static int sendSocket;
+static int client;
 
 int unix_uart_reset(struct UARTDriver *self) {
-
-
 	char * backend_socket_path = DEFAULT_SOCKET;
 	if (getenv(SOCKET_ENV)) {
-	backend_socket_path = getenv(SOCKET_ENV);
+		backend_socket_path = getenv(SOCKET_ENV);
 	}
+
 	/* Unlink if it exists */
 	unlink(backend_socket_path);
-	printf("using socket at '%s'\n", backend_socket_path);
+	printf("using socket '%s'\n", backend_socket_path);
 
 	/* Create socket */
 	backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
@@ -30,8 +30,8 @@ int unix_uart_reset(struct UARTDriver *self) {
 	/* 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';
+	strncpy(sa.sun_path, backend_socket_path, strlen(backend_socket_path));
+	sa.sun_path[strlen(backend_socket_path)+1] = '\0';
 	if (bind(backendSocket, (struct sockaddr *) &sa, sizeof(sa))) {
 		err(-1, "bind");
 	}
@@ -42,22 +42,29 @@ int unix_uart_reset(struct UARTDriver *self) {
 	}
 
 	while (1) {
-		sendSocket = accept(backendSocket, NULL, NULL);
-		if (sendSocket > 0)
+		client = accept(backendSocket, NULL, NULL);
+		if (client > 0)
 			break;
 	}
-
-	printf(" accpet() returned with %d\n", sendSocket);
+	printf(" accpet() returned with %d\n", client);
   return 0;
 }
 
 int unix_uart_write(struct UARTDriver *self, unsigned char c) {
-	printf("sending %c\n", c);
-	return send(sendSocket, &c, 1, 0);
+	return send(client, &c, 1, MSG_DONTWAIT);
+	// return write(client, &c, 1);
 }
 
 int unix_uart_read(struct UARTDriver *self, unsigned char *c) {
-	int bytes = read(sendSocket, c, 1);
-	// printf("read in %d byte [%x], msg=%d\n", bytes, *c, self->msg);
-	return bytes;
-}
+	int bytes_available;
+	ioctl(client,FIONREAD,&bytes_available);
+	
+	if (bytes_available > 0) {
+		int bytes = recv(client, c, 1, 0);
+		printf("read in %d byte [%x]\n", bytes, *c);
+		return bytes;
+	} else {
+		return 0;
+	}
+	
+}
\ No newline at end of file