Making code slightly more verbose and ready for testing with quad

groundStation/src/backend/backend.c

@@ -116,7 +116,7 @@ static void cb(struct ucart_vrpn_TrackerData * td) {
 	static int count = 0;
 
 	if(!(count % 10)) {
-		//sendVrpnPacket(td);
+		sendVrpnPacket(td);
 		//updateLogFile(td);
 	}
 	count++;
@@ -239,6 +239,8 @@ int main(int argc, char **argv)
 						 * Ignore stdin from the backend
 						 */
 					} else if (fd == zyboSocket) {
+						printf("recieving from quad\n");
+
 						/**
 						 * Read the response from the control loop
 						 */
@@ -256,7 +258,6 @@ int main(int argc, char **argv)
 						//findTimeDiff(id);
 
 						quad_recv(respBuf, respLen);
-
 						// if(respLen == 11) {
 						// 	int id = getInt((unsigned char *)respBuf, 7);
 						// 	findTimeDiff(id);
@@ -697,7 +698,7 @@ static void quad_recv(const char * buf, size_t len) {
 		onto  the clients, do so.
 	 */
 	
-	unsigned char packet[len];
+	char packet[len];
 	int validPacket;
 	unsigned char *data;
 	metadata_t metadata;
@@ -705,15 +706,28 @@ static void quad_recv(const char * buf, size_t len) {
 	memcpy(packet, buf, len);
 	
 	// Validate the message is correctly formatted
-	validPacket = parse_packet(packet, &data, &metadata);
+	validPacket = parse_packet((unsigned char *) packet, &data, &metadata);
 	if(validPacket != 0) {
 		warnx("Could not recognize packet from quad.\n");
 		return;
 	}
 
+	int datalen = (packet[6] << 8) | (packet[5]);
 	char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText;
-	float value = getFloat(packet, 7);
+	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);
+		}
+	}
 }
 
 static int wasDisconnected(int fd) {

groundStation/src/backend/commands.h

@@ -94,7 +94,7 @@ int respthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
 int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
 int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
 int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getrollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
+int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
 
 float getFloat(unsigned char* str, int pos);
 int getInt(unsigned char* str, int pos);