loging wifi latency

groundStation/src/backend/backend.c

@@ -114,7 +114,6 @@ 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);
@@ -233,31 +232,32 @@ int main(int argc, char **argv)
 					if(wasDisconnected(fd)){
 						break;
 					}
-
 					if (fd == fileno(stdin)) {
 						/**
 						 * Ignore stdin from the backend
 						 */
 					} else if (fd == zyboSocket) {
-						printf("recieving from quad\n");
-
+						//printf("recieving from quad\n");
 						/**
 						 * Read the response from the control loop
 						 */
 						int available;
 						ioctl(fd, FIONREAD, &available);
 						if (available < 12) {
+							//printf("only %d available\n", available);
 							continue;
 						}
+
+						//printf("recieving more than 12\n");
 						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);
+						int id = getInt((unsigned char *)respBuf, 7);
+						findTimeDiff(id);
 
-						quad_recv(respBuf, respLen);
+						//quad_recv(respBuf, respLen);
 						// if(respLen == 11) {
 						// 	int id = getInt((unsigned char *)respBuf, 7);
 						// 	findTimeDiff(id);
@@ -337,19 +337,27 @@ void sendStartPacket() {
 }
 
 void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
-	int pSize = sizeof(struct ucart_vrpn_TrackerData) + 8;
+	int pSize = 36;//sizeof(struct ucart_vrpn_TrackerData) + 8;
 	int n;
 	unsigned char packet[pSize];
 	currMessageID++;
 	packet[0] = 0xBE; // BEGIN					//PACKET_START_BYTE;
-	packet[1] = 0x04;	// UPDATE				//'U'; // U for vrpn camera update, C for command
+	packet[1] = 0x04; // UPDATE				//'U'; // U for vrpn camera update, C for command
 	packet[2] = 0x00; // N/A
 	//TODO Figure out Packet ID with this new ucar_vrpn_TrackerData struct
 	packet[3] = (currMessageID & 0x000000ff); 			// MSG ID(1)
 	packet[4] =	((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
-	packet[5] = (sizeof(struct ucart_vrpn_TrackerData) & 0x000000ff); 			// DATALEN(1)
-	packet[6] = ((sizeof(struct ucart_vrpn_TrackerData) >> 8) & 0x00000ff); 	// DATALEN(2)
-	memcpy(&packet[7], &info, sizeof(struct ucart_vrpn_TrackerData));
+	packet[5] = (28 & 0x000000ff); 			// DATALEN(1)
+	packet[6] = ((28) >> 8 & 0x00000ff); 	// DATALEN(2)
+	//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);
 
 	char checksum = 0;
 	int i;
@@ -363,6 +371,7 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 		perror("vrpnhandler: ERROR writing to socket");
 		keepRunning = 0;
 	}
+
 	struct timeval tstart;
 	gettimeofday(&tstart, NULL);
 	timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
@@ -766,7 +775,8 @@ void findTimeDiff(int respID) {
 	struct timeval result, tend;
 	gettimeofday(&tend, NULL);
 	timeval_subtract(&result, &tend, &timeArr[respID%MAX_HASH_SIZE]);
-	printf("elapsed time = %ld ms\n", result.tv_usec/1000);
+	printf("(BackEnd): Elapsed time = %ld ms\n", result.tv_usec/1000);
+	// printf("print to log\n");
 	// char tmp[8];
 	// snprintf(tmp, 8, "%ld \tms\n", result.tv_usec/1000);
 	// writeStringToLog(tmp);