diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c index 5c4dc9fa6590757f051969825b6969c62bcf8fb5..42db4a653ea6d12db5ea562e070bf748184e36aa 100644 --- a/groundStation/src/backend/backend.c +++ b/groundStation/src/backend/backend.c @@ -69,7 +69,7 @@ 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); +static void quad_recv(); /* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */ static int wasDisconnected(int fd); @@ -103,7 +103,7 @@ fd_set rfds_master; int max_fd = 0; pthread_mutex_t quadResponseMutex, cliInputMutex ; -char *respBuf, *commandBuf; +unsigned char *commandBuf; int newQuadResponse = 0, newCliInput = 0; // Structures to be used throughout @@ -178,7 +178,6 @@ int main(int argc, char **argv) if ((zyboSocket = connectToZybo()) < 0) { perror("Error connecting to Quad..."); - free(respBuf); free(commandBuf); exit(1); } @@ -218,7 +217,6 @@ int main(int argc, char **argv) .tv_usec = 0 }; - respBuf = calloc(CMD_MAX_LENGTH, sizeof(unsigned char)); sleep(3); while(keepRunning) { @@ -240,33 +238,7 @@ int main(int argc, char **argv) */ } 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); + quad_recv(); } else if (fd == backendSocket) { int new_fd = 0; new_fd = accept(backendSocket, NULL, NULL); @@ -693,41 +665,77 @@ static void client_recv(int fd) { } } -static void quad_recv(const char * buf, size_t len) { +static void quad_recv() { /* 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; + static unsigned char respBuf[2048]; + static size_t respBufLen; + + /** + * Read the response from the control loop + */ + int respLen = readQuad((char *) respBuf + respBufLen, + CMD_MAX_LENGTH); + if(respLen <= 0) { + perror("ERROR reading from quad...\n"); + return; + } + respBufLen += respLen; + + if (respBufLen < 8) { + /* not long enough yet */ + return; + } - 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"); + validPacket = parse_packet((unsigned char *) respBuf, &data, &metadata); + if (validPacket == -1) { + warnx("Doesn't have start byte."); + /* nuke packet */ + respBufLen = 0; 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); + /* Get datalen */ + size_t datalen = metadata.data_len; + if (datalen > CMD_MAX_LENGTH - 8) { + /* Very invalid packet. Nuke that shit */ + respBufLen = 0; + return; + } + + if (respBufLen < datalen + 8) { + /* Packet not yet fully read */ + return; + } + + if (validPacket == 0) { + /* At least enough data read to check checksum, and it was good!*/ + char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText; + float value = getFloat((unsigned char *)respBuf, 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); + } } + } else { + warnx("Checksum mismatch!"); } + memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8)); + respBufLen -= datalen + 8; } static int wasDisconnected(int fd) { diff --git a/groundStation/src/backend/communication.c b/groundStation/src/backend/communication.c index 2dacd03f90afb8ed35b99a56f34ca785225e3350..19ea73ae4cf7c8f7f8a35bf8b316bf28e19e7903 100644 --- a/groundStation/src/backend/communication.c +++ b/groundStation/src/backend/communication.c @@ -236,8 +236,10 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met } // compare checksum - if(packet_checksum != calculated_checksum) + if(packet_checksum != calculated_checksum) { fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum); + return -2; + } return 0; }