Added missing files

groundStation/src/backend/communication.c

@@ -16,111 +16,8 @@ int formatCommand(char *command, unsigned char *formattedCommand) {
 
 	char * cmdText = strtok(cmd, " ");
 
-	metadata_t metadata;
-
-	// ----------------------------------------------
-	if(cmdText != NULL) {
-		for(int type = 0; type < MAX_TYPE_ID; type++)
-		{
-			if(strcmp(cmdText, MessageTypes[type].cmdText) == 0)
-			{
-				printf("Sending\n\ttype: %d, \n\tcommand: %s\n", type, MessageTypes[type].cmdText);
-				metadata.begin_char = (char) BEGIN_CHAR;
-				metadata.msg_type = type;
-				metadata.msg_id = msgNum++;
-				metadata.data_len = 0;
-
-
-				/* TODO: Format data correctly
-				 * - Implement a case for every type
-				 * 	- Format the tokens into data as appropriate for the type,
-				 *   	  based on the quad's callbacks.c.
-				 * - Pass the data and metadata into
-				 *   	  formatPacket
-				 * - Purge cmdDataType from existence
-				 */
-
-				/* Static data buffer */
-				char data[256];
-				switch (type) {
-					// In each case, us strtok(NULL, " ") to tokenize,
-					// format,
-					// and append to data buffer (increase
-					// metadat.data_len appropriately).
-					case DEBUG_ID:
-						break;
-					case PACKETLOG_ID:
-						break;
-					case GETPACKETLOGS_ID:
-						break;
-					case UPDATE_ID:
-						break;
-					case BEGINUPDATE_ID:
-						break;
-					case LOG_ID:
-						break;
-					case RESPONSE_ID:
-						break;
-					case SETCONTROL_ID:
-						break;
-					case GETCONTROL_ID:
-						break;
-					case RESPCONTROL_ID:
-						break;
-					default:
-						break;
-
-				}
-				return formatPacket(&metadata, data, formattedCommand);
-			}
-		}
-	}
-	// Only gets here if the command does not exist
 	return -1;	
 }
-// QUAD & Ground Station
-// Format the log data from log_message
-int formatPacket(metadata_t *metadata, void *data, unsigned char *formattedCommand)
-{
-	//----------------------------------------------------------------------------------------------
-	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
-	//---------------------------------------------------------------------------------------------|
-	// msg param|| beg char |               msg type       | msg id | data len (bytes) | data | checksum |
-	//-------------------------------------------------------------------------------------------- |
-	//	   bytes||	   1	|	  1	   |	  1		 |	  2	  |		   2		 | var	|	 1	   |
-	//----------------------------------------------------------------------------------------------
-
-	// Begin Char:
-	formattedCommand[0] = metadata->begin_char;
-
-	// Msg type:
-	formattedCommand[1] = metadata->msg_type;
-	formattedCommand[2] = ((metadata->msg_type >> 8) & 0x000000ff);
-
-	//Msg id (msgNum is 2 bytes)
-	formattedCommand[3] = (metadata->msg_id & 0x000000ff);
-	formattedCommand[4] = ((metadata->msg_id >> 8) & 0x000000ff);
-
-	// Data length and data - bytes 5&6 for len, 7+ for data
-	formattedCommand[5] = metadata->data_len & 0x000000ff;
-	formattedCommand[6] = (metadata->data_len >> 8) & 0x000000ff;
-
-	memcpy(&(formattedCommand[7]), data, metadata->data_len);
-
-	// Checksum
-	// receive data and calculate checksum
-	int i;
-	char data_checksum = 0;
-	for(i = 0; i < 7 + metadata->data_len; i++)
-	{
-		data_checksum ^= formattedCommand[i];
-	}
-
-	formattedCommand[7 + metadata->data_len] = data_checksum;
-
-	/* Return data length */
-	return metadata->data_len + 7;
-}
 
 // returns the length of the data in bytes (datalen from packet) and fills data
 // and metadata with the packet information

groundStation/src/backend/update.c

+#include "update.h"
+#include "commands.h"
+#include "bitwise.h"
+
+#include <sys/types.h>
+
+enum UpdateData {
+	ID_1,
+	ID_2,
+	ID_3,
+	ID_4,
+	Y_1,
+	Y_2,
+	Y_3,
+	Y_4,
+	X_1,
+	X_2,
+	X_3,
+	X_4,
+	Z_1,
+	Z_2,
+	Z_3,
+	Z_4,
+	ROLL_1,
+	ROLL_2,
+	ROLL_3,
+	ROLL_4,
+	PITCH_1,
+	PITCH_2,
+	PITCH_3,
+	PITCH_4,
+	YAW_1,
+	YAW_2,
+	YAW_3,
+	YAW_4,
+	UPDATE_SIZE
+};
+
+/* Creates data and metadata for an update packet
+ * Returns data size.
+ */
+ssize_t EncodeUpdate(
+        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 position_update * u)       /* Message to encode */
+{
+	m->msg_type = UPDATE_ID;
+	m->data_len = UPDATE_SIZE;
+
+	if (data_size < UPDATE_SIZE) {
+		return -1;
+	}
+
+	data[ID_1] = IntByte1(u->id);
+	data[ID_2] = IntByte2(u->id);
+	data[ID_3] = IntByte3(u->id);
+	data[ID_4] = IntByte4(u->id);
+
+	data[Y_1] = FloatByte1(u->y);
+	data[Y_2] = FloatByte2(u->y);
+	data[Y_3] = FloatByte3(u->y);
+	data[Y_4] = FloatByte4(u->y);
+
+	data[X_1] = FloatByte1(u->x);
+	data[X_2] = FloatByte2(u->x);
+	data[X_3] = FloatByte3(u->x);
+	data[X_4] = FloatByte4(u->x);
+
+	data[Z_1] = FloatByte1(u->z);
+	data[Z_2] = FloatByte2(u->z);
+	data[Z_3] = FloatByte3(u->z);
+	data[Z_4] = FloatByte4(u->z);
+
+	data[ROLL_1] = FloatByte1(u->roll);
+	data[ROLL_2] = FloatByte2(u->roll);
+	data[ROLL_3] = FloatByte3(u->roll);
+	data[ROLL_4] = FloatByte4(u->roll);
+
+	data[PITCH_1] = FloatByte1(u->pitch);
+	data[PITCH_2] = FloatByte2(u->pitch);
+	data[PITCH_3] = FloatByte3(u->pitch);
+	data[PITCH_4] = FloatByte4(u->pitch);
+
+	data[YAW_1] = FloatByte1(u->yaw);
+	data[YAW_2] = FloatByte2(u->yaw);
+	data[YAW_3] = FloatByte3(u->yaw);
+	data[YAW_4] = FloatByte4(u->yaw);
+
+	return UPDATE_SIZE;
+}
+
+/* Decode a metadata and data to populate an update.
+ * Returns 0 on success, -1 on failure.
+ */
+int DecodeUpdate(
+        struct position_update * u,     /* Decoded controller message */
+        const struct metadata * m,          /* Metadata to aid in decoding */
+        const uint8_t * data)               /* Data to decode */
+{
+	if (m->data_len < UPDATE_SIZE) {
+		return -1;
+	}
+	if (m->msg_type != UPDATE_ID) {
+		return -1;
+	}
+
+	u->id = BytesToInt(data[ID_1], data[ID_2], data[ID_3], data[ID_4]);
+	u->x = BytesToFloat(data[X_1], data[X_2], data[X_3], data[X_4]);
+	u->y = BytesToFloat(data[Y_1], data[Y_2], data[Y_3], data[Y_4]);
+	u->z = BytesToFloat(data[Z_1], data[Z_2], data[Z_3], data[Z_4]);
+
+	u->pitch = BytesToFloat(data[PITCH_1], data[PITCH_2], 
+			data[PITCH_3], data[PITCH_4]);
+	u->roll = BytesToFloat(data[ROLL_1], data[ROLL_2], 
+			data[ROLL_3], data[ROLL_4]);
+	u->yaw = BytesToFloat(data[YAW_1], data[YAW_2], 
+			data[YAW_3], data[YAW_4]);
+
+	return 0;
+}

groundStation/src/backend/update.h

+#ifndef _update_h
+#define _update_h
+
+#include "packet.h"
+
+#include <sys/types.h>
+
+struct position_update {
+	uint32_t id;
+	float x;
+	float y;
+	float z;
+	float pitch;
+	float roll;
+	float yaw;
+};
+
+/* Creates data and metadata for an update packet.
+ * Returns data size.
+ */
+ssize_t EncodeUpdate(
+		struct metadata *m, /* Out */
+		uint8_t *data,      /* Out */
+		size_t data_size,   /* Data buffer max size */
+		const struct position_update *u); /* In */
+
+/* Decode a metadata and data to populate an update
+ * Returns 0 on success, -1 on failure
+ */
+int DecodeUpdate(
+		struct position_update *u, /* Out */
+		const struct metadata *m,      /* In */
+		const uint8_t * data);         /* In */
+
+
+#endif