diff --git a/quad/sw/modular_quad_pid/src/commands.c b/quad/sw/modular_quad_pid/src/commands.c
index 3f2a62e83e23e12c5bb03f47e011d7999d359c4e..471b09b68080194d5e11978f9b2bf84ca9ab87a3 100644
--- a/quad/sw/modular_quad_pid/src/commands.c
+++ b/quad/sw/modular_quad_pid/src/commands.c
@@ -33,7 +33,29 @@ struct MessageType MessageTypes[MAX_TYPE] =
// Type of the command data
intType,
// Function pointer
- NULL
+ &debug
+ },
+ // Log packet receiving
+ {
+ // ID
+ 0x02,
+ // Command text
+ "packetlog",
+ // Type of the command data
+ stringType,
+ // Function pointer
+ &packetlog
+ },
+ // Get packet receiving logs
+ {
+ // ID
+ 0x03,
+ // Command text
+ "getpacketlogs",
+ // Type of the command data
+ stringType,
+ // Function pointer
+ &getpacketlogs
}
}
},
@@ -371,6 +393,25 @@ int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
+static int n_msg_received = 0;
+static size_t total_payload_received = 0;
+
+int packetlog(unsigned char* packet, int dataLen, modular_structs_t* structs) {
+ n_msg_received += 1;
+ total_payload_received += dataLen;
+ return 0;
+}
+
+int getpacketlogs(unsigned char* packet, int dataLen, modular_structs_t* structs) {
+ char buf[255];
+
+ // Message logging number of messages received and size of payload received
+ int length = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
/* Handles receiving new location updates */
int update(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
diff --git a/quad/sw/modular_quad_pid/src/commands.h b/quad/sw/modular_quad_pid/src/commands.h
index cf234f0f894fd95b9e3e347cab1ba3d779fcf45c..45be51e8cd10c623fabcf0dd8fd08d2744ac9e93 100644
--- a/quad/sw/modular_quad_pid/src/commands.h
+++ b/quad/sw/modular_quad_pid/src/commands.h
@@ -41,6 +41,8 @@ struct MessageType{
};
int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
+int packetlog(unsigned char* packet, int dataLen, modular_structs_t* structs);
+int getpacketlogs(unsigned char* packet, int dataLen, modular_structs_t* structs);
int update(unsigned char *c, int dataLen, modular_structs_t *structs);
int beginupdate(unsigned char *c, int dataLen, modular_structs_t *structs);
int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
diff --git a/quad/sw/modular_quad_pid/src/uart.c b/quad/sw/modular_quad_pid/src/uart.c
index d4bf4ed444526605f1325eb66cb0ed032881da9d..a31256e43d0afa9c1e30b5248ad558f04c0bbe8c 100644
--- a/quad/sw/modular_quad_pid/src/uart.c
+++ b/quad/sw/modular_quad_pid/src/uart.c
@@ -378,107 +378,4 @@ char uart1_recvByte() {
}
/************************************************/
-/************************************************/
-
-int tryReceivePacket(stringBuilder_t* sb, int echo) {
- while(uart0_hasData()) {
- char c = uart0_recvByte(); // begin char
- if(c == 0xBE) {
-// printf("Beginning to read packet from UART.\n");
-
- sb->addChar(sb, 0xBE);
- char type = uart0_recvByte();
- sb->addChar(sb, type); // type
- sb->addChar(sb, uart0_recvByte()); // subtype
- sb->addChar(sb, uart0_recvByte()); // id
- sb->addChar(sb, uart0_recvByte()); // id
-
- unsigned char byte5 = uart0_recvByte(); // data length
- unsigned char byte6 = uart0_recvByte(); // data length
-
- int datalen = (byte6 << 8) | byte5;
-// printf("Received packet with data length: %d", datalen);
-
- sb->addChar(sb, byte5);
- sb->addChar(sb, byte6);
-
- // Read all the data and the checksum byte
- int i;
- for(i=0; i < datalen + 1; i++)
- {
- sb->addChar(sb, uart0_recvByte());
- }
-
-// printf("Done reading packet from UART.\n");
- return type;
- } else {
-// printf("The first byte was not the begin char: %x\n", c);
- return -1;
- }
- }
- return -1;
-/*
-
- while (uart0_hasData()) {
- char c = uart0_recvByte();
- if (c == PACKET_START_CHAR) {
-#if DEBUG
- uart0_sendStr("Start ");
-#endif
- c = uart0_recvByte();
- char type = c;
-// sb->addChar(sb, type);
- int count = 0;
-
- //uart0_sendByte(c);
-
- // if it's a 'C' packet (a command for the quad), then
- // wait for the packet end char. Else, if it's a 'U' packet
- // which is an update from the camera system, then we read
- // a fixed number of bytes (UPDATE_SIZE # of bytes)
- if(type == 'C')
- {
- while (c != PACKET_END_CHAR)
- {
- c = uart0_recvByte();
- count++;
- if(c == PACKET_END_CHAR)
- break;
- if ((c != 0 && c != '\r' && c != '\n'))
- {
- sb->addChar(sb, c);
- if (echo) {
- uart0_sendByte(c);
- }
- }
- }
- return type;
- }
-
- else if(type == 'U')
- {
- while(count < UPDATE_SIZE)
- {
- c = uart0_recvByte();
- count++;
-
- sb->addChar(sb, c);
- if (echo) {
- uart0_sendByte(c);
- }
- }
- return type;
- }
-#if DEBUG
- uart0_sendStr("End:[");
- uart0_sendStr(sb->buf);
- uart0_sendStr("] ");
-#endif
- return 0;
- }
- }
-
- return 0;
- */
-}
diff --git a/quad/sw/modular_quad_pid/src/user_input.c b/quad/sw/modular_quad_pid/src/user_input.c
index 403aed279256eb1f321ae8d53d1e7ca981d1fc3f..bc0c4c5b7047afa4cdecf6093e48a6b977bf9a15 100644
--- a/quad/sw/modular_quad_pid/src/user_input.c
+++ b/quad/sw/modular_quad_pid/src/user_input.c
@@ -28,7 +28,7 @@ int get_user_input(log_t* log_struct, user_input_t* user_input_struct)
// then receive the packet and set hasPacket for later processing
// "update packet" type processing is done in sensor.c
// "command packet" type processing is done in control_algorithm.c
- user_input_struct->hasPacket = tryReceivePacket(user_input_struct->sb, 0);
+ //user_input_struct->hasPacket = tryReceivePacket(user_input_struct->sb, 0);
return 0;
}