diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index f4fcee7b039c73670146b11e78e427049010257b..1088a84cb0b96da0c343b742026c63223c3ec47d 100644
--- a/groundStation/src/backend/backend.c
+++ b/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);
}
diff --git a/quad/src/virt_quad/hw_impl_unix_uart.c b/quad/src/virt_quad/hw_impl_unix_uart.c
index 9d2d4df15633e01fda1484e184bd2fa99145db21..c4f024ff282e6038f4ef63bd88a615fc14e9673f 100644
--- a/quad/src/virt_quad/hw_impl_unix_uart.c
+++ b/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