Moved common stuff

groundStation/src/backend/common/bitwise.h → groundStation/src/backend/bitwise.h

@@ -8,4 +8,22 @@
 /* Build a 16-bit integer out of two bytes */
 #define BytesTo16(lsb, msb) ((lsb & 0xff) | ((msb << 8) & 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
+ */
+#define FloatByte1(x) (((char *) x)[0])
+#define FloatByte2(x) (((char *) x)[1])
+#define FloatByte3(x) (((char *) x)[2])
+#define FloatByte4(x) (((char *) x)[3])
+
+/* This is so much UB it hurts to write */
+#define BytesToFloat(f, b1, b2, b3, b4) \
+	do {                                \
+		((char *) f)[0] = b1;           \
+		((char *) f)[1] = b2;           \
+		((char *) f)[2] = b3;           \
+		((char *) f)[3] = b4;           \
+	} while (0);
+
 #endif

groundStation/src/backend/callbacks.h

@@ -5,8 +5,7 @@
 #include "type_def.h"
 
 /* Make commands.c happy */
-typedef void (command_cb)(unsigned char *command, 
-		int dataLen, modular_structs_t *structs);
+typedef void (command_cb)(void);
 
 float getFloat(unsigned char * str, int pos);
 

groundStation/src/backend/communication.c

@@ -188,8 +188,7 @@ int processCommand(unsigned char *packet, modular_structs_t *structs) {
 	}
 
 	if(metadata.data_len >= 0) {
-		(* (MessageTypes[(unsigned char)metadata.msg_type].functionPtr))(
-				data, metadata.data_len, structs);
+		(* (MessageTypes[(unsigned char)metadata.msg_type].functionPtr))();
 
 		return 0;
 	}

groundStation/src/backend/setcontrol.c

+#include "setcontrol.h"
+#include "../commands.h"
+
+#include <sys/types.h>
+
+enum SetcontrolData {
+	CTRL_ID,
+	CTRLVAL_ID,
+	VAL_1,
+	VAL_2,
+	VAL_3,
+	VAL_4,
+	SC_DATA_SIZE
+};
+
+/* Creates data and metadata for a setcontrol packet
+ * Returns data size.
+ */
+ssize_t EncodeSetcontrol(
+        struct metadata * m,        /* data_len and msg_type will be populated*/
+        uint8_t * data,             /* Output buffer */
+        size_t data_size,           /* Max buffer size */
+        const struct controller_message * cm)       /* Message to encode */
+{
+	m->msg_type = SETCONTROL_ID;
+	m->data_len = SC_DATA_SIZE;
+
+	if (data_size < SC_DATA_SIZE) {
+		return -1;
+	}
+
+	data[CTRL_ID] = cm->id;
+	data[CTRLVAL_ID] = cm->value_id;
+	data[VAL_1] = FloatByte1(cm->value);
+	data[VAL_2] = FloatByte2(cm->value);
+	data[VAL_3] = FloatByte3(cm->value);
+	data[VAL_4] = FloatByte4(cm->value);
+
+	return SC_DATA_SIZE;
+}
+
+/* Decode a metadata and data to populate a controller.
+ * Returns 0 on success, -1 on failure.
+ */
+int DecodeSetcontrol(
+        struct controller_message * cm,     /* Decoded controller message */
+        const struct metadata * m,          /* Metadata to aid in decoding */
+        const uint8_t * data)               /* Data to decode */
+{
+	if (m->data_len < SC_DATA_SIZE) {
+		return -1;
+	}
+	if (m->msg_type != SETCONTROL_ID) {
+		return -1;
+	}
+
+	/* Would violate strict aliasing to work directly on the struct member */
+	float val;
+	BytesToFloat(val, data[VAL_1], data[VAL_2], data[VAL_3], data[VAL_4]);
+
+	cm->id = data[CTRL_ID];
+	cm->value_id = data[CTRLVAL_ID];
+	cm->value = val;
+
+	return 0;
+}