From 14a37d297e8b8a25c8a3f081d2e060db03c39fa8 Mon Sep 17 00:00:00 2001
From: James Talbert <jtalbert@iastate.edu>
Date: Mon, 15 Apr 2019 12:20:41 -0500
Subject: [PATCH] Added override command on quad.
---
quad/src/commands/commands.c | 11 ++++++
quad/src/commands/commands.h | 3 +-
quad/src/quad_app/callbacks.c | 40 ++++++++++++++++++++++
quad/src/quad_app/do_output_override.c | 41 +++++++++++++++++++++++
quad/src/quad_app/do_output_override.h | 35 +++++++++++++++++++
quad/src/quad_app/initialize_components.c | 6 ++++
quad/src/quad_app/quad_app.c | 2 ++
quad/src/quad_app/type_def.h | 11 ++++++
8 files changed, 148 insertions(+), 1 deletion(-)
create mode 100644 quad/src/quad_app/do_output_override.c
create mode 100644 quad/src/quad_app/do_output_override.h
diff --git a/quad/src/commands/commands.c b/quad/src/commands/commands.c
index 8dddcbf3c..766bb4a90 100644
--- a/quad/src/commands/commands.c
+++ b/quad/src/commands/commands.c
@@ -75,6 +75,8 @@ 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")));
+
/*
* Command structure.
@@ -259,6 +261,15 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
floatType,
// Function pointer
&cb_respaddnode
+ },
+ // OVERRIDE_OUTPUT
+ {
+ // Command text
+ "overrideoutput",
+ // Type of the command data
+ stringType,
+ // Function pointer
+ &cb_overrideoutput
}
};
diff --git a/quad/src/commands/commands.h b/quad/src/commands/commands.h
index 194bbaedc..226a9306f 100644
--- a/quad/src/commands/commands.h
+++ b/quad/src/commands/commands.h
@@ -53,7 +53,8 @@ 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
+ OUTPUT_OVERRIDE_ID, // 19 - Override the outputs from the controller and use provided values instead
+ MAX_TYPE_ID // 20 - Just used to keep track of the size. Must remain at the end
};
struct modular_structs;
diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index 8c06e3bdd..575e081ec 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -409,3 +409,43 @@ int cb_addnode(struct modular_structs *structs, struct metadata *meta, unsigned
send_data(structs->hardware_struct.comm.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, sizeof(resp_buf));
return 0;
}
+
+/*
+ * Updates the output override state. When overridden, the controller
+ * results are ignored in favor of the set values.
+ * This command requires both the override mode and the values to be set at the same time.
+ * It would be dangerous to enable override without guaranteeing good values.
+ * Expects the uart buffer to have data in the following format:
+ * |---------------------------------------------------------------------------------------------|
+ * | data index || 0 | 1 - 4 | 5 - 8 | 9 - 12 | 13 - 16 |
+ * |---------------------------------------------------------------------------------------------|
+ * | parameter || Mode | Throttle | Roll | Pitch | Yaw |
+ * |---------------------------------------------------------------------------------------------|
+ * | bytes || 1 | 4 | 4 | 4 | 4 |
+ * |---------------------------------------------------------------------------------------------|
+ *
+ * Mode: enumeration
+ * 0: No override (use control algorithm outputs)
+ * 1: Override (use provided values)
+ * Throttle: Floating point in range: [0,1] represents intended average motor output
+ * Roll: Floating point in range: [0,1] represents intended difference between left and right-side output
+ * Pitch: Floating point in range: [0,1] represents intended difference between front and back-side output
+ * Yaw: Floating point in range: [0,1] represents intended difference between output of the two diagonals
+ *
+ * Returns nothing.
+ */
+int cb_overrideoutput(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
+
+ override_t* override_info = &structs->override_struct;
+ int num_overrides = sizeof(structs->override_struct.values)
+ / sizeof(*structs->override_struct.values);
+
+ // Check if the data length is large enough
+ if (length < 1+num_overrides*4) {return -1;}
+ override_info->mode = data[0];
+ int i;
+ for (i = 0; i < num_overrides; i++) {
+ override_info->mode = build_float(&data[1+i*4]);
+ }
+ return 0;
+}
diff --git a/quad/src/quad_app/do_output_override.c b/quad/src/quad_app/do_output_override.c
new file mode 100644
index 000000000..2df131f3e
--- /dev/null
+++ b/quad/src/quad_app/do_output_override.c
@@ -0,0 +1,41 @@
+/*
+ * send_actuator_commands.c
+ *
+ * Created on: Feb 20, 2016
+ * Author: ucart
+ */
+
+#include "do_output_override.h"
+#include "util.h"
+
+#define THROTTLE 0
+#define ROLL 1
+#define PITCH 2
+#define YAW 3
+
+static int motor_min = 0.00000;
+static int motor_max = 1.00000;
+
+static double motor_clamp(double val) {
+ if (isnan(val)) { val = motor_min; }
+ else if (val < motor_min) {val = motor_min;}
+ else if (val > motor_max) {val = motor_max;}
+ return val;
+}
+
+int do_output_override(log_t* log_struct, override_t* override_struct, actuator_command_t* actuator_command_struct) {
+ // write the PWMs to the motors
+ if (override_struct->mode == OVERRIDE_ALL) {
+ float *inputs = override_struct->values;
+ float motor0 = inputs[THROTTLE] - inputs[PITCH] - inputs[ROLL] - inputs[YAW];
+ float motor1 = inputs[THROTTLE] + inputs[PITCH] - inputs[ROLL] + inputs[YAW];
+ float motor2 = inputs[THROTTLE] - inputs[PITCH] + inputs[ROLL] + inputs[YAW];
+ float motor3 = inputs[THROTTLE] + inputs[PITCH] + inputs[ROLL] - inputs[YAW];
+ actuator_command_struct->motor_magnitudes[0] = motor_clamp(motor0);
+ actuator_command_struct->motor_magnitudes[1] = motor_clamp(motor1);
+ actuator_command_struct->motor_magnitudes[2] = motor_clamp(motor2);
+ actuator_command_struct->motor_magnitudes[3] = motor_clamp(motor3);
+ }
+
+ return 0;
+}
diff --git a/quad/src/quad_app/do_output_override.h b/quad/src/quad_app/do_output_override.h
new file mode 100644
index 000000000..7aef3dc19
--- /dev/null
+++ b/quad/src/quad_app/do_output_override.h
@@ -0,0 +1,35 @@
+/*
+ * send_actuator_commands.h
+ *
+ * Created on: Feb 20, 2016
+ * Author: ucart
+ */
+
+#ifndef DO_OUTPUT_OVERRIDE_H_
+#define DO_OUTPUT_OVERRIDE_H_
+
+#include <stdio.h>
+
+#include "log_data.h"
+#include "type_def.h"
+
+/**
+ * @brief
+ * Sends commands to the actuators.
+ *
+ * @param log_struct
+ * structure of the data to be logged
+ *
+ * @param actuator_command_struct
+ * structure of the commmands to go to the actuators
+ *
+ * @param actuator_command_struct
+ * structure of the commmands to go to the actuators
+ *
+ * @return
+ * error message
+ *
+ */
+int do_output_override(log_t* log_struct, override_t* override_struct, actuator_command_t* actuator_command_struct);
+
+#endif /* DO_OUTPUT_OVERRIDE_H_ */
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index edccbbbf8..3c8aba206 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -106,5 +106,11 @@ int init_structs(modular_structs_t *structs) {
sensor_processing_init(&structs->sensor_struct);
+ structs->override_struct.mode = OVERRIDE_NONE;
+ int num_overrides = sizeof(structs->override_struct.values)
+ / sizeof(*structs->override_struct.values);
+ for (i = 0; i < num_overrides; i++) {
+ structs->override_struct.values[i] = 0;
+ }
return 0;
}
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index a098f374a..f3b284ef8 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -13,6 +13,7 @@
#include "sensor_processing.h"
#include "control_algorithm.h"
#include "send_actuator_commands.h"
+#include "do_output_override.h"
#include "communication.h"
#include "mio7_led.h"
@@ -64,6 +65,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
+ do_output_override(&(structs.log_struct), &(structs.override_struct), &(structs.actuator_command_struct));
// send the actuator commands
send_actuator_commands(&(structs.hardware_struct.motors), &(structs.log_struct), &(structs.actuator_command_struct));
}
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 4d61cb1f0..f45743664 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -501,6 +501,16 @@ typedef struct hardware_t {
struct OpticalFlowDriver of;
} hardware_t;
+typedef enum {
+ OVERRIDE_NONE, // 0 Use controller output
+ OVERRIDE_ALL // 1 Use override values
+} override_mode_t;
+
+typedef struct override_t {
+ override_mode_t mode;
+ float values[4];
+} override_t;
+
/**
* @brief
* Structures to be used throughout
@@ -516,6 +526,7 @@ typedef struct modular_structs {
raw_actuator_t raw_actuator_struct;
actuator_command_t actuator_command_struct;
hardware_t hardware_struct;
+ override_t override_struct;
} modular_structs_t;
//////// END MAIN MODULAR STRUCTS
--
GitLab