changes to quad_recv

groundStation/src/backend/backend.c

@@ -34,6 +34,8 @@
 #include "vrpn_tracker.hpp"
 #include "type_def.h"
 #include "logger.h"
+#include "packet.h"
+#include "respcontrol.h"
 #include "config.h"
 
 #define QUAD_BT_ADDR  "00:06:66:64:61:D6"
@@ -272,80 +274,40 @@ int main(int argc, char **argv)
 }
 
 void sendStartPacket() {
-	unsigned char packet[8] = {0};
-	currMessageID++;
-	metadata_t metadata = 
-	{
-		(char) BEGIN_CHAR,
-		BEGINUPDATE_ID,
-		1,
-		0
-	};
+	uint8_t packet[64];
+	struct metadata m;
+	m.msg_type = BEGINUPDATE_ID;
+	m.data_len = 0;
+	m.msg_id = currMessageID++;
 
-	packet[0] = metadata.begin_char; // BEGIN					//PACKET_START_BYTE;
-	packet[1] = metadata.msg_type & 0xff;	// UPDATE				//'U'; // U for vrpn camera update, C for command
-	packet[2] = (metadata.msg_type >> 8) & 0xff; // BEGIN UPDATE
-	packet[3] = (currMessageID & 0x000000ff); 			// MSG ID(1)
-	packet[4] =	((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
-	packet[5] = 0; 			// DATALEN(1)
-	packet[6] = 0; 	// DATALEN(2)
-	
-	char checksum = 0;
-	int i;
-	for(i=0; i < metadata.data_len + 7; i++)
-		checksum ^= packet[i];
-
-	packet[metadata.data_len + 7] = checksum; //PACKET_END_BYTE;
-	int status = writeQuad((char * ) packet, metadata.data_len + 8);
-	if (status != 8)
-	{
-		perror("Error sending start packet...\n");
-		keepRunning = 0;
-	}else
-	{
-		printf("Start packet successfuly sent...\n");
-	}	
+	size_t psize;
+
+	if ((psize = EncodePacket(packet, 64, &m, NULL)) < 0) {
+		warnx("Big problems");
+		return;
+	}
+
+	writeQuad(packet, psize);
 }
 
 void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
-	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[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] = (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->y), 4);
-	memcpy(&packet[15], &(info->x), 4);
-	memcpy(&packet[19], &(info->z), 4);
-	memcpy(&packet[23], &(info->roll), 4);
-	memcpy(&packet[27], &(info->pitch), 4);
-	memcpy(&packet[31], &(info->yaw), 4);
-
-	char checksum = 0;
-	int i;
-	for(i=0; i < pSize - 1; i++) 
-		checksum ^= packet[i];
-
-	packet[pSize - 1] = checksum; //PACKET_END_BYTE;
-
-	n = writeQuad((char *) packet, pSize);
-	if(n < 0) {	
-		perror("vrpnhandler: ERROR writing to socket");
-		keepRunning = 0;
+	uint8_t packet[64];
+	struct metadata m;
+	uint8_t data[128];
+
+	if (EncodeUpdate(&m, data, 128, info)) {
+		warnx("Big problems");
+		return;
 	}
+	m.msg_id = currMessageID++;
 
-	struct timeval tstart;
-	gettimeofday(&tstart, NULL);
-	timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
+	size_t psize;
+	if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
+		warnx("Big problems");
+		return;
+	}
+
+	writeQuad(packet, psize);
 }
 
 void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
@@ -681,91 +643,59 @@ static void quad_recv() {
 		onto  the clients, do so.
 	 */
 	
-	int validPacket;
-	unsigned char data[256];
-	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) {
+	struct metadata m;
+	uint8_t data[256];
+	size_t respLen;
+
+	respLen = readQuad((char *) respBuf + respBufLen, 
+			CMD_MAX_LENGTH- respBufLen);
+	if (respLen <= 0) {
 		perror("ERROR reading from quad...\n");
 		return;
 	}
 	respBufLen += respLen;
 
-	if (respBufLen < 8) {
-		/* not long enough yet */
-		printf("too short!!\n");
-		return;
-	}
-
-	// Validate the message is correctly formatted
-	validPacket = parse_packet((unsigned char *) respBuf, data, &metadata);
-	if (validPacket == -1) {
-		warnx("Doesn't have start byte.");
-		/* nuke packet */
+	if (DecodePacket(&m, data, 256, respBuf, respBufLen) < 0) {
+		warnx("Packet format error");
 		respBufLen = 0;
 		return;
 	}
-	
-	/* Get datalen */
-	size_t datalen = metadata.data_len;
-	if (respBufLen < datalen + 8) {
-		/* Packet not yet fully read */
-		return;
-	}
+	memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
+	respBufLen -= PacketSize(&m);
 
-	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;
+	switch (m.msg_type) {
+		case RESPCONTROL_ID:
+			handleRespcontrol;
+			break;
+		case LOG_ID:
+			/* something like this */
+			log_write((char *) data, m.data_len);
+			break;
 	}
+}
 
-	if(validPacket == -2) {
-		fprintf(stderr, "Checksums did not match (Parse Packet)\n");
+static void handleRespcontrol(struct metadata *m, uint8_t * data)
+{
+	struct controller_message cm;
+	if (DecodeRespcontrol(&cm, m, data) < 0) {
+		warnx("Respcontrol error")
 		return;
 	}
 
-	if (validPacket == 0) {
-		/* At least enough data read to check checksum, and it was good!*/
-		char * cmdText = MessageTypes[(int)metadata.msg_type].cmdText;
-
-		/* TODO: Parse data correctly
-		 *  - Switch on msg_type
-		 *     - Parse data appropriately and format into a string
-		 *  - Send append data string to cmdText and send to clients
-		 */
-		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[128];
 
-		char buffer[1048];
-		sprintf(buffer, "%s %lf\n", cmdText, value);
+	char * message = cmToString(RESPCONTROL_ID, cm.id, cm.value_id);
 
-		for(int fd = 0; fd <= max_fd; ++fd) {
-			if (get_client_index(fd) > -1) {
-				write(fd, buffer, datalen + 8);
-			}
+	size_t len = snprintf(buffer, 128, "%s %f\n", message, cm.value);
+
+	for(int fd = 0; fd <= max_fd; ++fd) {
+		if (get_client_index(fd) > -1) {
+			write(fd, buffer, len);
 		}
 	}
-	memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
-	respBufLen -= datalen + 8;
 }
 
 static int wasDisconnected(int fd) {

groundStation/src/backend/packet.c

@@ -93,3 +93,8 @@ uint8_t PacketChecksum(const uint8_t * packet, size_t packet_size)
 	/* TODO implement */
 	return 42;
 }
+
+size_t PacketSize(const struct metadata *m)
+{
+	return m.data_len + HDR_SIZE + CSUM_SIZE;
+}
\ No newline at end of file