From dae1711fcbc7f43040747f44b9a0d47030ec1a0e Mon Sep 17 00:00:00 2001
From: ucart <ucart_groundstation@iastate.edu>
Date: Fri, 19 Apr 2019 21:45:14 -0500
Subject: [PATCH] Added Override command for characterization testing
---
groundStation/src/backend/backend.c | 4 ++
groundStation/src/backend/override.c | 51 +++++++++++++++++++
groundStation/src/backend/override.h | 18 +++++++
groundStation/src/cli/cli.c | 4 +-
groundStation/src/cli/cli.h | 8 ++-
groundStation/src/cli/cli_override.c | 34 +++++++++++++
groundStation/src/cli/cli_override.h | 8 +++
groundStation/src/frontend/frontend_common.h | 8 +++
.../src/frontend/frontend_override.c | 42 +++++++++++++++
.../src/frontend/frontend_override.h | 25 +++++++++
quad/src/commands/commands.c | 12 +++++
quad/src/commands/commands.h | 5 +-
12 files changed, 213 insertions(+), 6 deletions(-)
create mode 100644 groundStation/src/backend/override.c
create mode 100644 groundStation/src/backend/override.h
create mode 100644 groundStation/src/cli/cli_override.c
create mode 100644 groundStation/src/cli/cli_override.h
create mode 100644 groundStation/src/frontend/frontend_override.c
create mode 100644 groundStation/src/frontend/frontend_override.h
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 01928622a..c0043b256 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -875,6 +875,10 @@ static void client_recv(int fd) {
case ADDNODE_ID:
result = EncodeAddNode(&m, data, 128, buffer);
break;
+ case OUTPUT_OVERRIDE_ID:
+ result = EncodeSetOutputOverride(&m, data, 128, buffer);
+ break;
+
default:
result = -1;
break;
diff --git a/groundStation/src/backend/override.c b/groundStation/src/backend/override.c
new file mode 100644
index 000000000..e51c09496
--- /dev/null
+++ b/groundStation/src/backend/override.c
@@ -0,0 +1,51 @@
+#include <sys/types.h>
+#include <inttypes.h>
+#include <string.h>
+
+#include "output.h"
+#include "commands.h"
+#include "bitwise.h"
+
+#define NUM_OUTPUTS 4
+#define DATA_SIZE (1+NUM_OUTPUTS*4)
+
+/* Creates data and metadata for a overrideoutput packet
+ * Returns data size.
+ */
+ssize_t EncodeSetOutputOverride(
+ struct metadata * m, /* data_len and msg_type will be populated*/
+ uint8_t * data, /* Output buffer */
+ size_t data_size, /* Max buffer size */
+ const char * msg) /* Message to encode */
+{
+ m->msg_type = OUTPUT_OVERRIDE_ID;
+ m->data_len = DATA_SIZE;
+
+ if (data_size < DATA_SIZE) {
+ return -1;
+ }
+
+ float values[NUM_OUTPUTS];
+ values[0] = -1;
+ char* tok = strtok(msg, " ");
+ char* ena = strtok(NULL, " ");
+ data[0] = atoi(ena);
+ int i;
+ for (i = 0; i < NUM_OUTPUTS; i++) {
+ char* value = strtok(NULL, " ");
+ if (value == NULL) {
+ return -1;
+ }
+ float a;
+ sscanf(value, "%f", &a);
+ values[i] = a;
+ }
+
+ for (i = 0; i < NUM_OUTPUTS; i++) {
+ data[1+i*4+0] = FloatByte1(values[i]);
+ data[1+i*4+1] = FloatByte2(values[i]);
+ data[1+i*4+2] = FloatByte3(values[i]);
+ data[1+i*4+3] = FloatByte4(values[i]);
+ }
+ return DATA_SIZE;
+}
diff --git a/groundStation/src/backend/override.h b/groundStation/src/backend/override.h
new file mode 100644
index 000000000..f07fa6b41
--- /dev/null
+++ b/groundStation/src/backend/override.h
@@ -0,0 +1,18 @@
+#ifndef _OVERRIDE_h
+#define _OVERRIDE_h
+
+#include "packet.h"
+
+#include <sys/types.h>
+
+
+/* Creates data and metadata for a overrideoutput packet.
+ * Returns data size.
+ */
+ssize_t EncodeSetOutputOverride(
+ struct metadata *m, /* Out */
+ uint8_t *data, /* Out */
+ size_t data_size, /* Data buffer max size */
+ const char * msg); /* Value is not used; only IDs */
+
+#endif
diff --git a/groundStation/src/cli/cli.c b/groundStation/src/cli/cli.c
index 139534a47..6f258f2e2 100644
--- a/groundStation/src/cli/cli.c
+++ b/groundStation/src/cli/cli.c
@@ -57,7 +57,7 @@ int main(int argc, char **argv)
for (i = 0; i < MAX_COMMANDS; ++i) {
if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0) {
cmdID = i;
- }
+ }
}
}
@@ -233,4 +233,4 @@ int convert_to_id(struct backend_conn * conn, char **argv, struct convert_data *
}
frontend_free_node_data(search_data, num_nodes);
return search_2;
-}
\ No newline at end of file
+}
diff --git a/groundStation/src/cli/cli.h b/groundStation/src/cli/cli.h
index f91e40fbc..7dca99d74 100644
--- a/groundStation/src/cli/cli.h
+++ b/groundStation/src/cli/cli.h
@@ -7,6 +7,7 @@
#include "cli_output.h"
#include "cli_nodes.h"
#include "cli_trackable.h"
+#include "cli_override.h"
struct backend_conn;
@@ -20,6 +21,7 @@ enum CommandNameIds{
CMD_ADDNODE,
CMD_GETTRACKABLE,
CMD_SETTRACKABLE,
+ CMD_OUTPUTOVERRIDE,
MAX_COMMANDS
};
@@ -33,7 +35,8 @@ static cli_function_ptr cli_functions[] = {
&cli_getnodes,
&cli_addnode,
&cli_gettrackable,
- &cli_settrackable
+ &cli_settrackable,
+ &cli_outputoverride
};
static char* commandNames[MAX_COMMANDS] = {
@@ -45,7 +48,8 @@ static char* commandNames[MAX_COMMANDS] = {
"getnodes",
"addnode",
"gettrackable",
- "settrackable"
+ "settrackable",
+ "outputoverride"
};
/**
diff --git a/groundStation/src/cli/cli_override.c b/groundStation/src/cli/cli_override.c
new file mode 100644
index 000000000..8ee4fe283
--- /dev/null
+++ b/groundStation/src/cli/cli_override.c
@@ -0,0 +1,34 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include "cli.h"
+#include "cli_override.h"
+#include "frontend_override.h"
+
+int cli_outputoverride(struct backend_conn * conn, int argc, char ** argv) {
+ int needHelp = 0;
+ struct frontend_override_data values;
+
+ if ((needHelp = help_check(argc, argv))) {
+ printf("outputoverride sets the override state of the hardware platform\n");
+ printf("Usage Syntax : \n\t./Cli outputoverride <enable> Throttle Pitch Roll Yaw \n");
+ return 0;
+ }
+
+ if (argc < 6) {
+ printf("Incorrect Usage, run './cli outputoverride --help' for correct usage.\n");
+ return 1;
+ }
+ int ena;
+ sscanf(argv[1], "%d", &ena); values.enable = ena;
+ sscanf(argv[2], "%f", &values.throttle);
+ sscanf(argv[3], "%f", &values.pitch);
+ sscanf(argv[4], "%f", &values.roll);
+ sscanf(argv[5], "%f", &values.yaw);
+
+ if (frontend_setoutputoverride(conn, &values)) {
+ return 1;
+ }
+ return 0;
+}
diff --git a/groundStation/src/cli/cli_override.h b/groundStation/src/cli/cli_override.h
new file mode 100644
index 000000000..ebb8340c5
--- /dev/null
+++ b/groundStation/src/cli/cli_override.h
@@ -0,0 +1,8 @@
+#ifndef _CLI_OVERRIDE_H
+#define _CLI_OVERRIDE_H
+
+#include "frontend_output.h"
+
+int cli_outputoverride(struct backend_conn * conn, int argc, char ** argv);
+
+#endif /* _CLI_OVERRIDE_H */
diff --git a/groundStation/src/frontend/frontend_common.h b/groundStation/src/frontend/frontend_common.h
index 827802875..27983cb08 100644
--- a/groundStation/src/frontend/frontend_common.h
+++ b/groundStation/src/frontend/frontend_common.h
@@ -56,6 +56,14 @@ struct frontend_node_data {
char *name;
};
+struct frontend_override_data {
+ int8_t enable;
+ float throttle;
+ float pitch;
+ float roll;
+ float yaw;
+};
+
#ifdef __cplusplus
}
#endif
diff --git a/groundStation/src/frontend/frontend_override.c b/groundStation/src/frontend/frontend_override.c
new file mode 100644
index 000000000..aac819df9
--- /dev/null
+++ b/groundStation/src/frontend/frontend_override.c
@@ -0,0 +1,42 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <err.h>
+#include <inttypes.h>
+
+#include "frontend_override.h"
+
+/* Set the override state of outputs
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_setoutputoverride(
+ struct backend_conn * conn,
+ struct frontend_override_data * values) {
+
+ char msg[64] = "";
+ int written;
+
+ snprintf(msg, 64, "outputoverride %" PRId8 " %f %f %f %f\n",
+ values->enable,
+ values->throttle, values->pitch, values->roll, values->yaw);
+
+ if((written = ucart_backendWrite(conn, msg)) < 0) {
+ return 1;
+ }
+
+ size_t pendingResponses = 0;
+ char * response;
+
+ while (pendingResponses) {
+ response = ucart_backendGetline(conn);
+ if (response == NULL) {
+ warnx("Line not returned from backend");
+ return 1;
+ }
+
+
+ }
+
+ return 0;
+}
diff --git a/groundStation/src/frontend/frontend_override.h b/groundStation/src/frontend/frontend_override.h
new file mode 100644
index 000000000..53f016d32
--- /dev/null
+++ b/groundStation/src/frontend/frontend_override.h
@@ -0,0 +1,25 @@
+/**
+ * frontend_output writes the getoutput command to the backend connection.
+ */
+
+#ifndef _FRONTEND_OVERRIDE_H
+#define _FRONTEND_OVERRIDE_H
+
+#include "frontend_common.h"
+
+/* Set the override state of outputs
+ *
+ * Returns 0 on success, 1 on error
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+int frontend_setoutputoverride(
+ struct backend_conn * conn,
+ struct frontend_override_data * values);
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif /* __FRONTEND_OVERRIDE_H */
diff --git a/quad/src/commands/commands.c b/quad/src/commands/commands.c
index 8dddcbf3c..9942b62c9 100644
--- a/quad/src/commands/commands.c
+++ b/quad/src/commands/commands.c
@@ -74,6 +74,7 @@ command_cb cb_respnodes __attribute__((weak, alias("cb_default")));
command_cb cb_addnode __attribute__((weak, alias("cb_default")));
command_cb cb_respaddnode __attribute__((weak, alias("cb_default")));
+command_cb cb_overrideoutput __attribute__((weak, alias("cb_default")));
/*
@@ -259,7 +260,18 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
floatType,
// Function pointer
&cb_respaddnode
+ },
+ // OVERRIDE_OUTPUT
+ {
+ // Command text
+ "outputoverride",
+ // Type of the command data
+ stringType,
+ // Function pointer
+ &cb_overrideoutput
}
+
+
};
int findCommand(char * str)
diff --git a/quad/src/commands/commands.h b/quad/src/commands/commands.h
index 41dcb96f3..731eb0d29 100644
--- a/quad/src/commands/commands.h
+++ b/quad/src/commands/commands.h
@@ -53,8 +53,9 @@ enum MessageTypeID{
RESPNODES_ID, // 16 - Responding with node IDs from current comp_graph
ADDNODE_ID, // 17 - Add a node of specified type_id
RESPADDNODE_ID, // 18 - Responding with the block_id of the newly added node
- MAX_TYPE_ID, // 19 - Just used to keep track of the size. Must remain at the end
- SEND_RT_ID // 20 - Real Time sensor data sent to groundstation
+ OUTPUT_OVERRIDE_ID, // 19 - Override the outputs from the controller and use provided values instead
+ SEND_RT_ID, // 20 - Real Time sensor data sent to groundstation
+ MAX_TYPE_ID // 21 - Just used to keep track of the size. Must remain at the end
};
struct modular_structs;
--
GitLab