Added Override command for characterization testing

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;

groundStation/src/backend/override.c

+#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;
+}

groundStation/src/backend/override.h

+#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

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
+}

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"
 };
 
 /**

groundStation/src/cli/cli_override.c

+#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;
+}

groundStation/src/cli/cli_override.h

+#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 */

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

groundStation/src/frontend/frontend_override.c

+#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;
+}

groundStation/src/frontend/frontend_override.h

+/**
+ * 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 */

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)

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;