From a8aaabf92a9cdd9f294b3b167eeff1b0bd8b5525 Mon Sep 17 00:00:00 2001
From: Jake Drahos <j@kedrahos.com>
Date: Thu, 2 Feb 2017 01:10:30 -0600
Subject: [PATCH] Added missing files
---
groundStation/src/backend/communication.c | 103 ------------------
groundStation/src/backend/update.c | 121 ++++++++++++++++++++++
groundStation/src/backend/update.h | 36 +++++++
3 files changed, 157 insertions(+), 103 deletions(-)
create mode 100644 groundStation/src/backend/update.c
create mode 100644 groundStation/src/backend/update.h
diff --git a/groundStation/src/backend/communication.c b/groundStation/src/backend/communication.c
index 924f7d829..2a6a920e4 100644
--- a/groundStation/src/backend/communication.c
+++ b/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
diff --git a/groundStation/src/backend/update.c b/groundStation/src/backend/update.c
new file mode 100644
index 000000000..db546e05a
--- /dev/null
+++ b/groundStation/src/backend/update.c
@@ -0,0 +1,121 @@
+#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;
+}
diff --git a/groundStation/src/backend/update.h b/groundStation/src/backend/update.h
new file mode 100644
index 000000000..783a861ca
--- /dev/null
+++ b/groundStation/src/backend/update.h
@@ -0,0 +1,36 @@
+#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
--
GitLab