Implemented update encoding/decoding

groundStation/src/backend/backend.c

@@ -35,6 +35,7 @@
 #include "type_def.h"
 #include "packet.h"
 #include "respcontrol.h"
+#include "update.h"
 #include "config.h"
 #include "cmHandler.h"
 
@@ -317,13 +318,22 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
 	struct metadata m;
 	uint8_t data[128];
 
-	if (EncodeUpdate(&m, data, 128, info)) {
+	struct position_update u;
+	u.id = currMessageID;
+	u.x = info->x;
+	u.y = info->y;
+	u.z = info->z;
+	u.pitch = info->pitch;
+	u.roll = info->roll;
+	u.yaw = info->yaw;
+
+	if (EncodeUpdate(&m, data, 128, &u)) {
 		warnx("Big problems");
 		return;
 	}
 	m.msg_id = currMessageID++;
 
-	size_t psize;
+	ssize_t psize;
 	if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
 		warnx("Big problems");
 		return;
@@ -650,7 +660,7 @@ static void client_recv(int fd) {
 				printf(" 0x%.2x", (signed) packet[i]);
 			}
 			printf("'\n");
-			writeQuad((char *) packet, packetSize);
+			writeQuad(packet, packetSize);
 		}
 		
 		char * rest = &buffer[newline] + 1;

groundStation/src/backend/bitwise.h

@@ -11,6 +11,15 @@
 /* Build a 16-bit integer out of two bytes */
 #define BytesTo16(lsb, msb) (((lsb) & 0xff) | (((msb) << 8) & 0xff))
 
+/* Same for 32bit */
+#define IntByte1(x) ((x) & 0xff)
+#define IntByte2(x) (((x) >> 8) & 0xff)
+#define IntByte3(x) (((x) >> 16) & 0xff)
+#define IntByte4(x) (((x) >> 24) & 0xff)
+
+#define BytesToInt(b1, b2, b3, b4) (((b1) & 0xff) | (((b2) << 8) & 0xff) \
+		| (((b3) << 16) & 0xff) | (((b4) & 0xff)))
+
 /* Break apart a float. Sadly this is UB, but the "correct" way
  * to do this involves actually implementing
  * IEEE 754 in software to do so

groundStation/src/backend/cmHandler.c

 #include "cmHandler.h"
 
-/*enum respContIndices {
+/*
+enum respContIndices {
 	GET_PITCH_P ,
 	GET_PITCH_I ,
 	GET_PITCH_D ,
@@ -30,11 +31,10 @@
 	GET_LONG_I	,
 	GET_LONG_D	,
 	GET_LONG	,
-	GET_LONG_RATE,
 	MAX_COMMANDS
-};*/
+};
 
-const char * respContStrings[MAX_COMMANDS] {
+const char * respContStrings[MAX_COMMANDS] = {
 	"getrollp",
 	"getrolli",
 	"getrolld",
@@ -71,7 +71,8 @@ const char * respContStrings[MAX_COMMANDS] {
 	"getheighti",
 	"getheightd",
 	"getheight"
-}
+};
+*/
 
 const char * cmToString(int msgType, const struct controller_message *cm) 
 {
@@ -88,7 +89,8 @@ const char * cmToString(int msgType, const struct controller_message *cm)
 			return NULL;
 			break;
 		case RESPCONTROL_ID:
-			return respContStrings[cm->id + cm->value_id];
+			return "FOOBAR";
+//			return respContStrings[cm->id + cm->value_id];
 			break;
 		default:
 			return NULL;