Working groundstation and partially working auto. data dump

groundStation/src/backend/backend.c

@@ -195,7 +195,6 @@ int main(int argc, char **argv)
 		perror("Error creating log file...");
 		exit(1);
 	}
-	// writeStringToLog(logHeader);
 
 	// watch for input from stdin (fd 0) to see when it has input
 	safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
@@ -328,8 +327,8 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 	memcpy(&packet[11], &(info->y), 4);
 	memcpy(&packet[15], &(info->x), 4);
 	memcpy(&packet[19], &(info->z), 4);
-	memcpy(&packet[23], &(info->pitch), 4);
-	memcpy(&packet[27], &(info->roll), 4);
+	memcpy(&packet[23], &(info->roll), 4);
+	memcpy(&packet[27], &(info->pitch), 4);
 	memcpy(&packet[31], &(info->yaw), 4);
 
 	char checksum = 0;
@@ -482,6 +481,9 @@ static ssize_t writeQuad(const char * buf, size_t count) {
 
 static ssize_t readQuad(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__);
 	}
@@ -709,17 +711,23 @@ static void quad_recv() {
 		respBufLen = 0;
 		return;
 	}
-
+	
 	/* Get datalen */
 	size_t datalen = metadata.data_len;
+	if (respBufLen < datalen + 8) {
+		/* Packet not yet fully read */
+		return;
+	}
+
 	if (datalen > CMD_MAX_LENGTH - 8) {
 		/* Very invalid packet. Nuke that shit */
+		warnx("data len is over the maximum packet length. Try reducing the data in the packet sent to the groundstation.\n");
 		respBufLen = 0;
 		return;
 	}
-	
-	if (respBufLen < datalen + 8) {
-		/* Packet not yet fully read */
+
+	if(validPacket == -2) {
+		fprintf(stderr, "Checksums did not match (Parse Packet)\n");
 		return;
 	}
 
@@ -728,10 +736,18 @@ static void quad_recv() {
 		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.
 		   */
+		if(strncmp(cmdText, "log", strlen(cmdText)) == 0) {
+			char log_text[datalen+1];
+			strncpy(log_text, (char *) data, datalen);
+			log_text[datalen] = '\0';
+			printf("log='%s'\n", log_text);
+		}
+
 		char buffer[1048];
 		sprintf(buffer, "%s %lf\n", cmdText, value);
 
@@ -740,8 +756,6 @@ static void quad_recv() {
 				write(fd, buffer, datalen + 8);
 			}
 		}
-	} else {
-		warnx("Checksum mismatch!");
 	}
 	memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
 	respBufLen -= datalen + 8;

groundStation/src/backend/communication.c

@@ -230,14 +230,13 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met
 
 	// calculate checksum
 	unsigned char calculated_checksum = 0;
-	for(i = 0; i < meta_data->data_len + 7; i++)
+	for(i = 0; i < meta_data->data_len + 7 - 1; i++)
 	{
 		calculated_checksum ^= packet[i];
 	}
 
 	// compare 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;
 	}