Changed how packets are received

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) {

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;
 }