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

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

+#include "node_mixer.h"
+#include <stdlib.h>
+
+static int pwm_min = 100000;
+static int pwm_max = 200000;
+
+static int pwm_clamp(int val) {
+	if (val < pwm_min) {val = pwm_min;}
+	if (val > pwm_max) {val = pwm_max;}
+	return val;
+}
+
+static void mixer_computation(void *state, const double* params, const double *inputs, double *outputs) {
+	int pwm0 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] - inputs[MIXER_ROLL] - inputs[MIXER_YAW];
+	int pwm1 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] - inputs[MIXER_ROLL] + inputs[MIXER_YAW];
+	int pwm2 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] + inputs[MIXER_ROLL] + inputs[MIXER_YAW];
+	int pwm3 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] + inputs[MIXER_ROLL] - inputs[MIXER_YAW];
+	outputs[MIXER_PWM0] = pwm_clamp(pwm0);
+	outputs[MIXER_PWM1] = pwm_clamp(pwm1);
+	outputs[MIXER_PWM2] = pwm_clamp(pwm2);
+	outputs[MIXER_PWM3] = pwm_clamp(pwm3);
+}
+
+static void reset_mixer(void *state) {}
+
+static const char* const in_names[4] = {"Throttle", "Pitch", "Roll", "Yaw"};
+static const char* const out_names[4] = {"PWM 0", "PWM 1", "PWM 2", "PWM 3"};
+static const char* const param_names[0] = {};
+const struct graph_node_type node_mixer_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 4,
+        .n_outputs = 4,
+        .n_params = 0,
+        .execute = mixer_computation,
+        .reset = reset_mixer
+};
+
+int graph_add_node_mixer(struct computation_graph *graph, const char* name) {
+    return graph_add_node(graph, name, &node_mixer_type, NULL);
+}

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

+#ifndef __NODE_MIXER_H__
+#define __NODE_MIXER_H__
+#include "../computation_graph.h"
+
+int graph_add_node_mixer(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_mixer_type;
+
+enum graph_node_mixer_inputs {
+    MIXER_THROTTLE,
+    MIXER_PITCH,
+    MIXER_ROLL,
+    MIXER_YAW,
+};
+
+enum graph_node_mixer_outputs {
+    MIXER_PWM0,
+    MIXER_PWM1,
+    MIXER_PWM2,
+    MIXER_PWM3,
+};
+
+#endif // __NODE_MIXER_H__

quad/sw/modular_quad_pid/src/type_def.h

@@ -314,6 +314,14 @@ typedef struct parameter_t {
 	int rc_throttle;
 	// Loop time
 	int dt;
+	// Signal mixer
+	int mixer;
+	// Clamping blocks
+	int clamp_pitch;
+	int clamp_roll;
+	int clamp_d_pwmR; // Clamp the change in PWM values for roll
+	int clamp_d_pwmP; // ... pitch
+	int clamp_d_pwmY; // ... yaw
 } parameter_t;
 
 /**