Network now handles signal mixing, and does clamping. Test flight passes.

quad/computation_graph/src/computation_graph.c

@@ -162,6 +162,10 @@ void graph_compute_node_rec(struct computation_graph *graph, int node_id, int de
         if (src_cntl_id != -1) {
             exec_input_vals[input_id] = graph->nodes[src_cntl_id].output_values[src_output_id];
         }
+        else {
+        	// Set input value to 0 if not connected
+        	exec_input_vals[input_id] = 0;
+        }
     }
     if (node->type->execute != NULL) {
         (*node->type->execute)(node->state, node->param_values, exec_input_vals, node->output_values);

quad/computation_graph/src/graph_blocks/node_accumulator.c

+#include "node_accumulator.h"
+#include <stdlib.h>
+
+struct accum_state {
+    double accumulated;
+};
+
+static void accum_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+    struct accum_state* my_state = (struct accum_state*)state;
+    my_state->accumulated += inputs[ACCUM_IN];
+    outputs[ACCUMULATED] = my_state->accumulated;
+}
+
+static void reset(void *state) {
+    ((struct accum_state*)state)->accumulated = 0;
+}
+
+
+static const char* const in_names[2] = {"Accumulator in"};
+static const char* const out_names[1] = {"Accumulated"};
+static const char* const param_names[0] = {};
+const struct graph_node_type node_accum_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 1,
+        .n_outputs = 1,
+        .n_params = 0,
+        .execute = accum_nodes,
+        .reset = reset
+};
+
+int graph_add_node_accum(struct computation_graph *graph, const char* name) {
+    struct accum_state* node_state = malloc(sizeof(struct accum_state));
+    if (sizeof(struct accum_state) && !node_state) {
+        return -1; // malloc failed
+    }
+    return graph_add_node(graph, name, &node_accum_type, node_state);
+}

quad/computation_graph/src/graph_blocks/node_accumulator.h

+#ifndef __NODE_ACCUMULATOR_H__
+#define __NODE_ACCUMULATOR_H__
+#include "../computation_graph.h"
+
+int graph_add_node_accum(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_accum_type;
+
+enum graph_node_accum_inputs {
+    ACCUM_IN,
+};
+
+enum graph_node_accum_outputs {
+    ACCUMULATED
+};
+#endif // __NODE_ACCUMULATOR_H__

quad/computation_graph/src/graph_blocks/node_add.c

+#include "node_add.h"
+#include <stdlib.h>
+
+static void add_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+    outputs[ADD_SUM] = inputs[ADD_SUMMAND1] + inputs[ADD_SUMMAND2];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[2] = {"Summand 1", "Summand 2"};
+static const char* const out_names[1] = {"Sum"};
+static const char* const param_names[0] = {};
+const struct graph_node_type node_add_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 2,
+        .n_outputs = 1,
+        .n_params = 0,
+        .execute = add_nodes,
+        .reset = reset
+};
+
+int graph_add_node_add(struct computation_graph *graph, const char* name) {
+    return graph_add_node(graph, name, &node_add_type, NULL);
+}

quad/computation_graph/src/graph_blocks/node_add.h

+#ifndef __NODE_ADD_H__
+#define __NODE_ADD_H__
+#include "../computation_graph.h"
+
+int graph_add_node_add(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_add_type;
+
+enum graph_node_add_inputs {
+    ADD_SUMMAND1,
+    ADD_SUMMAND2,
+};
+
+enum graph_node_add_outputs {
+    ADD_SUM
+};
+#endif // __NODE_ADD_H__

quad/computation_graph/src/graph_blocks/node_constant.c

+#include "node_constant.h"
+#include <stdlib.h>
+
+static void output_const(void *state, const double *params, const double *inputs, double *outputs) {
+    outputs[CONST_VAL] = params[CONST_SET];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[0] = {};
+static const char* const out_names[1] = {"Constant"};
+static const char* const param_names[1] = {"Constant"};
+const struct graph_node_type node_const_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 0,
+        .n_outputs = 1,
+        .n_params = 1,
+        .execute = output_const,
+        .reset = reset
+};
+
+int graph_add_node_const(struct computation_graph *graph, const char* name) {
+    return graph_add_node(graph, name, &node_const_type, NULL);
+}

quad/computation_graph/src/graph_blocks/node_constant.h

+#ifndef __NODE_CONSTANT_H__
+#define __NODE_CONSTANT_H__
+#include "../computation_graph.h"
+
+int graph_add_node_const(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_const_type;
+
+enum graph_node_const_params {
+    CONST_SET
+};
+
+enum graph_node_const_outputs {
+    CONST_VAL
+};
+#endif //__NODE_CONSTANT_H__

quad/computation_graph/src/graph_blocks/node_gain.c

+#include "node_gain.h"
+#include <stdlib.h>
+
+static void scale_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+    outputs[GAIN_RESULT] = inputs[GAIN_INPUT] * params[GAIN_GAIN];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[1] = {"Input"};
+static const char* const out_names[1] = {"Amplified"};
+static const char* const param_names[1] = {"Gain"};
+const struct graph_node_type node_gain_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 1,
+        .n_outputs = 1,
+        .n_params = 1,
+        .execute = scale_nodes,
+        .reset = reset
+};
+
+int graph_add_node_gain(struct computation_graph *graph, const char* name) {
+    return graph_add_node(graph, name, &node_gain_type, NULL);
+}

quad/computation_graph/src/graph_blocks/node_gain.h

+#ifndef __NODE_GAIN_H__
+#define __NODE_GAIN_H__
+#include "../computation_graph.h"
+
+int graph_add_node_gain(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_gain_type;
+
+enum graph_node_pow_inputs {
+    GAIN_INPUT
+};
+
+enum graph_node_pow_params {
+    GAIN_GAIN
+};
+
+enum graph_node_gain_outputs {
+    GAIN_RESULT
+};
+#endif // __NODE_GAIN_H__

quad/computation_graph/src/graph_blocks/node_mult.c

+#include "node_mult.h"
+#include <stdlib.h>
+
+static void mult_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+    outputs[MULT_PRODUCT] = inputs[MULT_MULTIPLICAND1] * inputs[MULT_MULTIPLICAND2];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[2] = {"Multiplicand 1", "Multiplicand 2"};
+static const char* const out_names[1] = {"Product"};
+static const char* const param_names[0] = {};
+const struct graph_node_type node_mult_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 2,
+        .n_outputs = 1,
+        .n_params = 0,
+        .execute = mult_nodes,
+        .reset = reset
+};
+
+int graph_add_node_mult(struct computation_graph *graph, const char* name) {
+    return graph_add_node(graph, name, &node_mult_type, NULL);
+}

quad/computation_graph/src/graph_blocks/node_mult.h

+#ifndef __NODE_MULT_H__
+#define __NODE_MULT_H__
+#include "../computation_graph.h"
+
+int graph_add_node_mult(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_mult_type;
+
+enum graph_node_mult_inputs {
+    MULT_MULTIPLICAND1,
+    MULT_MULTIPLICAND2,
+};
+
+enum graph_node_mult_outputs {
+    MULT_PRODUCT
+};
+
+#endif  // __NODE_MULT_H__

quad/computation_graph/src/graph_blocks/node_pow.c

+#include "node_pow.h"
+#include <stdlib.h>
+#include <math.h>
+
+static void pow_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+    outputs[POW_RESULT] = pow(inputs[POW_BASE], params[POW_EXP]);
+}
+static void reset(void *state) {}
+
+static const char* const in_names[1] = {"Base"};
+static const char* const out_names[1] = {"Result"};
+static const char* const param_names[1] = {"Exponent"};
+const struct graph_node_type node_pow_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 1,
+        .n_outputs = 1,
+        .n_params = 1,
+        .execute = pow_nodes,
+        .reset = reset
+};
+
+int graph_add_node_pow(struct computation_graph *graph, const char* name) {
+    return graph_add_node(graph, name, &node_pow_type, NULL);
+}

quad/computation_graph/src/graph_blocks/node_pow.h

+#ifndef __NODE_POW_H__
+#define __NODE_POW_H__
+#include "../computation_graph.h"
+
+int graph_add_node_pow(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_pow_type;
+
+enum graph_node_pow_inputs {
+    POW_BASE
+};
+
+enum graph_node_pow_params {
+    POW_EXP
+};
+
+enum graph_node_pow_outputs {
+    POW_RESULT
+};
+#endif // __NODE_POW_H__

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -12,6 +12,7 @@
 #include "graph_blocks/node_pid.h"
 #include "graph_blocks/node_bounds.h"
 #include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_mixer.h"
 
 #define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
 
@@ -31,6 +32,13 @@
     ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
     ps->alt_pid = graph_add_node_pid(graph, "Altitude");
 
+    // Create blocks for bounds checking
+    ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp");
+    ps->clamp_d_pwmR = graph_add_node_bounds(graph, "R PWM Clamp");
+    ps->clamp_d_pwmY = graph_add_node_bounds(graph, "Y PWM Clamp");
+    ps->clamp_pitch = graph_add_node_bounds(graph, "Pitch Clamp");
+    ps->clamp_roll= graph_add_node_bounds(graph, "Roll Clamp");
+
     // Create blocks for sensor inputs
     ps->cur_pitch = graph_add_node_const(graph, "Pitch");
     ps->cur_roll = graph_add_node_const(graph, "Roll");
@@ -50,6 +58,8 @@
     ps->rc_yaw = graph_add_node_const(graph, "RC Yaw");
     ps->rc_throttle = graph_add_node_const(graph, "RC Throttle");
 
+    ps->mixer = graph_add_node_mixer(graph, "Signal Mixer");
+
     ps->dt = graph_add_node_const(graph, "dT");
 
     // Connect pitch PID chain
@@ -78,6 +88,17 @@
     graph_set_source(graph, ps->yaw_pid, PID_DT, ps->dt, CONST_VAL);
     */
 
+    // Connect PWM clamping blocks
+    graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->clamp_d_pwmR, BOUNDS_IN, ps->roll_r_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->clamp_d_pwmY, BOUNDS_IN, ps->yaw_r_pid, PID_CORRECTION);
+
+    // Connect signal mixer
+    graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
+    graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->clamp_d_pwmP, BOUNDS_OUT);
+    graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT);
+    graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
+
     // Set pitch PID constants
     graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
     graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
@@ -102,13 +123,27 @@
     graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI);
     graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD);
 
+    // Set angle clamping limits
+    graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
+    graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
+    graph_set_param_val(graph, ps->clamp_roll, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
+    graph_set_param_val(graph, ps->clamp_roll, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
+
+    // Set PWM difference clamping limits
+    graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MIN, -20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MAX, 20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MIN, -20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MAX, 20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, 20000);
+
     // TODO: Change this to use LOOP_TIME
     graph_set_param_val(graph, ps->dt, CONST_SET, 0.005);
 
 	return 0;
  }
 
- int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* parameter_struct, user_defined_t* user_defined_struct, raw_actuator_t* raw_actuator_struct, modular_structs_t* structs)
+ int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* parameter_struct, user_defined_t* user_defined_struct, actuator_command_t* actuator_struct, modular_structs_t* structs)
  {
     struct computation_graph* graph = parameter_struct->graph;
 	// use the 'flap' switch as the flight mode selector
@@ -226,6 +261,7 @@
     graph_set_param_val(graph, parameter_struct->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
     graph_set_param_val(graph, parameter_struct->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);
     graph_set_param_val(graph, parameter_struct->rc_yaw, CONST_SET, user_input_struct->yaw_manual_setpoint);
+    graph_set_param_val(graph, parameter_struct->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
 
 
 	/*parameter_struct->pid_controllers[PITCH_ID].setpoint =
@@ -236,14 +272,15 @@
 			(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
 			(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
     */
-
+    /*
     int outputs[3] = {parameter_struct->pitch_r_pid,
                              parameter_struct->roll_r_pid,
-                             parameter_struct->yaw_r_pid};
-    graph_compute_nodes(graph, outputs, 3);
+                             parameter_struct->yaw_r_pid};*/
+    int outputs[1] = {parameter_struct->mixer};
+    graph_compute_nodes(graph, outputs, 1);
 
 	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
-	memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
+	//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
 
 	// don't use the PID corrections if the throttle is less than about 10% of its range
 	if((user_input_struct->rc_commands[THROTTLE] >
@@ -282,20 +319,17 @@
 //					parameter_struct->pid_controllers[ALT_ID].setpoint, parameter_struct->pid_controllers[ALT_ID].current_point);
 		}
 		else{
-			//ROLL
-			raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
-                    graph_get_output(graph, parameter_struct->roll_r_pid, PID_CORRECTION);
+			//THROTTLE
+			actuator_struct->pwms[0] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM0);
 
-			//PITCH
-			raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
-                    graph_get_output(graph, parameter_struct->pitch_r_pid, PID_CORRECTION);
+			actuator_struct->pwms[1] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM1);
 
-			//YAW
-			raw_actuator_struct->controller_corrected_motor_commands[YAW] =
-                    graph_get_output(graph, parameter_struct->yaw_r_pid, PID_CORRECTION);
+			actuator_struct->pwms[2] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM2);
+
+			actuator_struct->pwms[3] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM3);
 		}
 
-        // TODO: Make these blocks
+        /*
 		//BOUNDS CHECKING
 		if(raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] < 0)
 			raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0;
@@ -318,13 +352,18 @@
 
 		if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -20000)
 			raw_actuator_struct->controller_corrected_motor_commands[YAW] = -20000;
-
+		*/
 	}
 	else
 	{
+		actuator_struct->pwms[0] = user_input_struct->rc_commands[THROTTLE];
+		actuator_struct->pwms[1] = user_input_struct->rc_commands[THROTTLE];
+		actuator_struct->pwms[2] = user_input_struct->rc_commands[THROTTLE];
+		actuator_struct->pwms[3] = user_input_struct->rc_commands[THROTTLE];
+		/*
 		raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 0;
 		raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 0;
-		raw_actuator_struct->controller_corrected_motor_commands[YAW] = 0;
+		raw_actuator_struct->controller_corrected_motor_commands[YAW] = 0;*/
 	}
 
 	//logging

quad/sw/modular_quad_pid/src/control_algorithm.h

@@ -57,7 +57,7 @@ int control_algorithm(log_t* log_struct,
                       setpoint_t* setpoint_struct, 
                       parameter_t* parameter_struct, 
                       user_defined_t* user_defined_struct, 
-                      raw_actuator_t* raw_actuator_struct,
+                      actuator_command_t* actuator_struct,
                       modular_structs_t* structs);
 
 /**

quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h

@@ -11,7 +11,7 @@ enum graph_node_bounds_inputs {
 };
 
 enum graph_node_bounds_outputs {
-    BOUNDS_OUT // Boundde output
+    BOUNDS_OUT // Bounded output
 };
 
 enum graph_node_bounds_params {

quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c

-#include "node_constant.h"
-#include <stdlib.h>
-
-static void output_const(void *state, const double *params, const double *inputs, double *outputs) {
-    outputs[CONST_VAL] = params[CONST_SET];
-}
-static void reset(void *state) {}
-
-static const char* const in_names[0] = {};
-static const char* const out_names[1] = {"Constant"};
-static const char* const param_names[1] = {"Constant"};
-const struct graph_node_type node_const_type = {
-        .input_names = in_names,
-        .output_names = out_names,
-        .param_names = param_names,
-        .n_inputs = 0,
-        .n_outputs = 1,
-        .n_params = 1,
-        .execute = output_const,
-        .reset = reset
-};
-
-int graph_add_node_const(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_const_type, NULL);
-}

quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c

+/local/ucart/MicroCART_17-18/quad/computation_graph/src/graph_blocks/node_constant.c
\ No newline at end of file

quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h

-#ifndef __NODE_CONSTANT_H__
-#define __NODE_CONSTANT_H__
-#include "../computation_graph.h"
-
-int graph_add_node_const(struct computation_graph *graph, const char* name);
-
-extern const struct graph_node_type node_const_type;
-
-enum graph_node_const_params {
-    CONST_SET
-};
-
-enum graph_node_const_outputs {
-    CONST_VAL
-};
-#endif //__NODE_CONSTANT_H__

quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h

+/local/ucart/MicroCART_17-18/quad/computation_graph/src/graph_blocks/node_constant.h
\ No newline at end of file