Added override command on quad.

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

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;

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

quad/src/quad_app/do_output_override.c

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

quad/src/quad_app/do_output_override.h

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

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

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));
 		}

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