Added echo functionality to parse_packet(). Sends back the message ID upon receiving a message.

modular_quad_pid/src/commands.c

@@ -22,6 +22,18 @@ struct MessageType MessageTypes[MAX_TYPE] =
 				stringType,
 				// Function pointer
 				&debug
+			},
+
+			// Echo subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"ack",
+				// Type of the command data
+				intType,
+				// Function pointer
+				NULL
 			}
 		}
 	},
@@ -388,6 +400,8 @@ int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
 // ------------------------------------------------------------------
 // Quad side implementation
 
+// TODO: Erase memory leaks
+
 int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
 {
 	float value;

modular_quad_pid/src/communication.c

@@ -113,6 +113,33 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met
 	if(packet_checksum != calculated_checksum)
 		printf("Checksums did not match (Quadlog): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
 
+	//////////////////////////////
+	// Send an acknowledgment packet
+
+	// Send a reply to the ground station
+
+	int buf = meta_data->msg_id;
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[0].ID,
+		MessageTypes[0].subtypes[1].ID,
+		0,
+		sizeof(int)
+	};
+	formatPacket(&metadata, &buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+	free(responsePacket);
+
 	return 0;
 }
 

modular_quad_pid/src/communication.h

@@ -7,6 +7,7 @@
 #include <limits.h>
 #include "commands.h"
 #include "type_def.h"
+#include "uart.h"
 
 int formatCommand(unsigned char *command, unsigned char **formattedCommand);
 int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);