Fixed VRPN packet to 100%

groundStation/src/backend/backend.c

@@ -115,6 +115,7 @@ modular_structs_t structs;
 // 		in the future.
 static void cb(struct ucart_vrpn_TrackerData * td) {
 	static int count = 0;
+
 	if(!(count % 10)) {
 		sendVrpnPacket(td);
 		//updateLogFile(td);
@@ -324,12 +325,12 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 	//memcpy(&packet[7], &info, sizeof(struct ucart_vrpn_TrackerData));
 
 	memset(&packet[7], 0, 4);
-	memcpy(&packet[11], &info[4], 4);
-	memcpy(&packet[15], &info[0], 4);
-	memcpy(&packet[19], &info[8], 4);
-	memcpy(&packet[23], &info[16], 4);
-	memcpy(&packet[27], &info[12], 4);
-	memcpy(&packet[31], &info[20], 4);
+	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[31], &(info->yaw), 4);
 
 	char checksum = 0;
 	int i;
@@ -344,12 +345,6 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 		keepRunning = 0;
 	}
 
-	printf("vrpn packet : '");
-	for(int i = 0; i < pSize; ++i) {
-		printf(" %0x ");
-	}
-	printf("'\n");
-
 	struct timeval tstart;
 	gettimeofday(&tstart, NULL);
 	timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
@@ -477,7 +472,6 @@ static ssize_t writeQuad(const char * buf, size_t count) {
 	if (pthread_mutex_lock(&quadSocketMutex)) {
 		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
 	}
-	//setsockopt(zyboSocket, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int));
 	retval = write(zyboSocket, buf, count);
 	if (pthread_mutex_unlock(&quadSocketMutex)) {
 		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
@@ -492,9 +486,6 @@ static ssize_t readQuad(char * buf, size_t count) {
 		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
 	}
 	retval = read(zyboSocket, buf, count);
-	//int result = setsockopt(zyboSocket, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int));
-	//printf("result = %d\n", result);
-		
 	if (pthread_mutex_unlock(&quadSocketMutex)) {
 		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
 	}
@@ -704,6 +695,7 @@ static void quad_recv() {
 
 	if (respBufLen < 8) {
 		/* not long enough yet */
+		printf("too short!!\n");
 		return;
 	}
 
@@ -733,7 +725,7 @@ static void quad_recv() {
 		/* 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);
+		//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.