From ad27a49f62cd37b09fb115dd5947971989a65954 Mon Sep 17 00:00:00 2001
From: Jake Drahos <j@kedrahos.com>
Date: Tue, 31 Jan 2017 15:16:42 -0600
Subject: [PATCH] Created getcontrol/respcontrol
Hopefully there are no copy-paste bugs
---
groundStation/src/backend/bitwise.h | 55 ++++++++++++++++-----
groundStation/src/backend/getcontrol.c | 65 +++++++++++++++++++++++++
groundStation/src/backend/getcontrol.h | 28 +++++++++++
groundStation/src/backend/respcontrol.c | 65 +++++++++++++++++++++++++
groundStation/src/backend/respcontrol.h | 28 +++++++++++
groundStation/src/backend/setcontrol.c | 6 +--
6 files changed, 230 insertions(+), 17 deletions(-)
create mode 100644 groundStation/src/backend/getcontrol.c
create mode 100644 groundStation/src/backend/getcontrol.h
create mode 100644 groundStation/src/backend/respcontrol.c
create mode 100644 groundStation/src/backend/respcontrol.h
diff --git a/groundStation/src/backend/bitwise.h b/groundStation/src/backend/bitwise.h
index fec707046..b82b71f11 100644
--- a/groundStation/src/backend/bitwise.h
+++ b/groundStation/src/backend/bitwise.h
@@ -1,6 +1,9 @@
#ifndef _bitwise_h
#define _bitwise_h
+#include <string.h>
+#include <stdint.h>
+
/* Bit shifting for endianness of 16-bit numbers */
#define LSByte16(x) ((x) & 0xff)
#define MSByte16(x) (((x) >> 8) & 0xff)
@@ -12,18 +15,44 @@
* 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);
+static inline uint8_t FloatByte1(float f)
+{
+ char arr[4];
+ memcpy(arr, &f, 4);
+ return arr[0];
+}
+
+static inline uint8_t FloatByte2(float f)
+{
+ char arr[4];
+ memcpy(arr, &f, 4);
+ return arr[1];
+}
+
+static inline uint8_t FloatByte3(float f)
+{
+ char arr[4];
+ memcpy(arr, &f, 4);
+ return arr[2];
+}
+
+static inline uint8_t FloatByte4(float f)
+{
+ char arr[4];
+ memcpy(arr, &f, 4);
+ return arr[3];
+}
+
+static inline float BytesToFloat(uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4)
+{
+ char arr[4];
+ arr[0] = b1;
+ arr[1] = b2;
+ arr[2] = b3;
+ arr[3] = b4;
+ float f;
+ memcpy(&f, arr, 4);
+ return f;
+}
#endif
diff --git a/groundStation/src/backend/getcontrol.c b/groundStation/src/backend/getcontrol.c
new file mode 100644
index 000000000..460f3a038
--- /dev/null
+++ b/groundStation/src/backend/getcontrol.c
@@ -0,0 +1,65 @@
+#include "getcontrol.h"
+#include "commands.h"
+#include "bitwise.h"
+
+#include <sys/types.h>
+
+enum GetcontrolData {
+ CTRL_ID,
+ CTRLVAL_ID,
+ VAL_1,
+ VAL_2,
+ VAL_3,
+ VAL_4,
+ GC_DATA_SIZE
+};
+
+/* Creates data and metadata for a respcontrol packet
+ * Returns data size.
+ */
+ssize_t EncodeGetcontrol(
+ 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 = GETCONTROL_ID;
+ m->data_len = GC_DATA_SIZE;
+
+ if (data_size < GC_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 GC_DATA_SIZE;
+}
+
+/* Decode a metadata and data to populate a controller.
+ * Returns 0 on success, -1 on failure.
+ */
+int DecodeGetcontrol(
+ 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 < GC_DATA_SIZE) {
+ return -1;
+ }
+ if (m->msg_type != GETCONTROL_ID) {
+ return -1;
+ }
+
+
+ cm->id = data[CTRL_ID];
+ cm->value_id = data[CTRLVAL_ID];
+ cm->value = BytesToFloat(data[VAL_1], data[VAL_2],
+ data[VAL_3], data[VAL_4]);
+
+ return 0;
+}
diff --git a/groundStation/src/backend/getcontrol.h b/groundStation/src/backend/getcontrol.h
new file mode 100644
index 000000000..edbb85081
--- /dev/null
+++ b/groundStation/src/backend/getcontrol.h
@@ -0,0 +1,28 @@
+#ifndef _getcontrol_h
+#define _getcontrol_h
+
+#include "packet.h"
+#include "controller.h"
+
+#include <sys/types.h>
+
+
+/* Creates data and metadata for a getcontrol packet.
+ * Returns data size.
+ */
+ssize_t EncodeGetcontrol(
+ struct metadata *m, /* Out */
+ uint8_t *data, /* Out */
+ size_t data_size, /* Data buffer max size */
+ const struct controller_message *cm); /* Value is not used; only IDs */
+
+/* Decode a metadata and data to populate a message
+ * Returns 0 on success, -1 on failure
+ */
+int DecodeGetcontrol(
+ struct controller_message *cm, /* Out. Value is undefined */
+ const struct metadata *m, /* In */
+ const uint8_t * data); /* In */
+
+
+#endif
diff --git a/groundStation/src/backend/respcontrol.c b/groundStation/src/backend/respcontrol.c
new file mode 100644
index 000000000..78c3f290c
--- /dev/null
+++ b/groundStation/src/backend/respcontrol.c
@@ -0,0 +1,65 @@
+#include "respcontrol.h"
+#include "commands.h"
+#include "bitwise.h"
+
+#include <sys/types.h>
+
+enum RespcontrolData {
+ CTRL_ID,
+ CTRLVAL_ID,
+ VAL_1,
+ VAL_2,
+ VAL_3,
+ VAL_4,
+ RC_DATA_SIZE
+};
+
+/* Creates data and metadata for a respcontrol packet
+ * Returns data size.
+ */
+ssize_t EncodeRespcontrol(
+ 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 = RESPCONTROL_ID;
+ m->data_len = RC_DATA_SIZE;
+
+ if (data_size < RC_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 RC_DATA_SIZE;
+}
+
+/* Decode a metadata and data to populate a controller.
+ * Returns 0 on success, -1 on failure.
+ */
+int DecodeRespcontrol(
+ 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 < RC_DATA_SIZE) {
+ return -1;
+ }
+ if (m->msg_type != RESPCONTROL_ID) {
+ return -1;
+ }
+
+
+ cm->id = data[CTRL_ID];
+ cm->value_id = data[CTRLVAL_ID];
+ cm->value = BytesToFloat(data[VAL_1], data[VAL_2],
+ data[VAL_3], data[VAL_4]);
+
+ return 0;
+}
diff --git a/groundStation/src/backend/respcontrol.h b/groundStation/src/backend/respcontrol.h
new file mode 100644
index 000000000..a23c22550
--- /dev/null
+++ b/groundStation/src/backend/respcontrol.h
@@ -0,0 +1,28 @@
+#ifndef _respcontrol_h
+#define _respcontrol_h
+
+#include "packet.h"
+#include "controller.h"
+
+#include <sys/types.h>
+
+
+/* Creates data and metadata for a respcontrol packet.
+ * Returns data size.
+ */
+ssize_t EncodeRespcontrol(
+ struct metadata *m, /* Out */
+ uint8_t *data, /* Out */
+ size_t data_size, /* Data buffer max size */
+ const struct controller_message *cm); /* In */
+
+/* Decode a metadata and data to populate a message
+ * Returns 0 on success, -1 on failure
+ */
+int DecodeRespcontrol(
+ struct controller_message *cm, /* Out */
+ const struct metadata *m, /* In */
+ const uint8_t * data); /* In */
+
+
+#endif
diff --git a/groundStation/src/backend/setcontrol.c b/groundStation/src/backend/setcontrol.c
index 536c69001..2d32e224a 100644
--- a/groundStation/src/backend/setcontrol.c
+++ b/groundStation/src/backend/setcontrol.c
@@ -55,13 +55,11 @@ int DecodeSetcontrol(
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;
+ cm->value = BytesToFloat(data[VAL_1], data[VAL_2],
+ data[VAL_3], data[VAL_4]);
return 0;
}
--
GitLab