vrpn sending works with joe's new command structure

groundStation/src/backend/backend.c

@@ -231,9 +231,11 @@ int main(int argc, char **argv)
 	quadlog_file = fopen(log_file, "a");
 
 
+
 	// Tell the quad we are ready to send it vrpn data
 	sendStartPacket();
 
+
 	if(!getenv(NOVRPN_ENV)){
 		// create vrpnTracker instance
 		tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
@@ -306,11 +308,12 @@ void sendStartPacket() {
 	ssize_t psize;
 
 	if ((psize = EncodePacket(packet, 64, &m, NULL)) < 0) {
-		warnx("Big problems");
+		warnx("Big problems. sendStartPacket");
 		return;
 	}
 
 	writeQuad(packet, psize);
+	printf("Start Packet sent...\n");
 }
 
 void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
@@ -327,15 +330,15 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 	u.roll = info->roll;
 	u.yaw = info->yaw;
 
-	if (EncodeUpdate(&m, data, 128, &u)) {
-		warnx("Big problems");
+	if (EncodeUpdate(&m, data, 128, &u) < 0) {
+		warnx("Big problems. sendVrpnPacket . EncodeUpdate");
 		return;
 	}
 	m.msg_id = currMessageID++;
 
 	ssize_t psize;
 	if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
-		warnx("Big problems");
+		warnx("Big problems. sendVrpnPacket. EncodePacket");
 		return;
 	}
 
@@ -671,28 +674,38 @@ static void client_recv(int fd) {
 }
 
 static void quad_recv() {
-	static unsigned char respBuf[2048];
+	static unsigned char respBuf[4096];
 	static size_t respBufLen;
 
 	struct metadata m;
-	uint8_t data[1024];
+	uint8_t data[4096];
 	size_t respLen;
 	ssize_t datalen;
 	size_t packetlen;
 
 	respLen = readQuad((char *) respBuf + respBufLen, 
-			CMD_MAX_LENGTH- respBufLen);
+			CMD_MAX_LENGTH - respBufLen);
 	if (respLen <= 0) {
 		perror("ERROR reading from quad...\n");
 		return;
 	}
 	respBufLen += respLen;
-
-	if ((datalen = DecodePacket(&m, data, 1024, respBuf, respBufLen)) < 0) {
-		warnx("Packet format error");
+	datalen = DecodePacket(&m, data, 2048, respBuf, respBufLen);
+	
+	if (datalen == -1) {
+		warnx("No start Byte!\n");
 		respBufLen = 0;
 		return;
 	}
+	if (datalen == -5) {
+		warnx("Chechsum mismatch!\n");
+		respBufLen = 0;
+		return;
+	}
+	if (datalen < 0){
+		/* Not enough data yet. We need to wait for more */
+		return;	
+	}
 
 	packetlen = PacketSize(&m);
 	memmove(respBuf, respBuf + packetlen, respBufLen - packetlen);

groundStation/src/backend/bitwise.h

@@ -9,7 +9,7 @@
 #define MSByte16(x) (((x) >> 8) & 0xff)
 
 /* Build a 16-bit integer out of two bytes */
-#define BytesTo16(lsb, msb) (((lsb) & 0xff) | (((msb) << 8) & 0xff))
+#define BytesTo16(lsb, msb) (((lsb) & 0xff) | (((msb) & 0xff) << 8))
 
 /* Same for 32bit */
 #define IntByte1(x) ((x) & 0xff)

groundStation/src/backend/packet.c

@@ -61,37 +61,43 @@ ssize_t DecodePacket(
         const uint8_t * packet,     /* Packet to decode */
         size_t packet_size)         /* Size of packet to decode */
 {
+	uint8_t checkSum;
 	if (packet[BEGIN] != BEGIN_CHAR) {
 		return -1;
 	}
 
 	if (packet_size < ((uint8_t) HDR_SIZE + CSUM_SIZE)) {
-		return -1;
+		return -2;
 	}
 
 	m->msg_type = BytesTo16(packet[MTYPE_L], packet[MTYPE_H]);
 	m->msg_id = BytesTo16(packet[ID_L], packet[ID_H]);
-	m->data_len = BytesTo16(packet[DLEN_L], packet[DLEN_H]);
+	m->data_len =  BytesTo16(packet[DLEN_L], packet[DLEN_H]);
 
 	if (packet_size < (HDR_SIZE + CSUM_SIZE + m->data_len)) {
-		return -1;
+		return -3;
 	}
 
 	if (data_size < m->data_len) {
-		return -1;
+		return -4;
 	}
 
-	memcpy(data, &packet[HDR_SIZE], m->data_len);
+	checkSum = PacketChecksum(packet, packet_size);
+	if (checkSum != packet[HDR_SIZE + m->data_len]) {
+		return -5;
+	}
 
-	/* Validate checksum */
-	/* TODO */
+	memcpy(data, &packet[HDR_SIZE], m->data_len);
 	return m->data_len;
 }
 
 uint8_t PacketChecksum(const uint8_t * packet, size_t packet_size)
 {
-	/* TODO implement */
-	return 42;
+	uint8_t checkSum = 0;
+	for(size_t i = 0; i < packet_size - CSUM_SIZE; i++){
+		checkSum ^= packet[i];
+	}	
+	return checkSum;
 }
 
 size_t PacketSize(const struct metadata *m)