From e6f8920edccc0039d8e7ba7a9aa5111632177257 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Sat, 11 Feb 2017 18:16:35 -0600
Subject: [PATCH] Network now handles signal mixing, and does clamping. Test
flight passes.
---
.../computation_graph/src/computation_graph.c | 4 +
.../src/graph_blocks/node_accumulator.c | 39 ++++++++++
.../src/graph_blocks/node_accumulator.h | 16 ++++
.../src/graph_blocks/node_add.c | 25 +++++++
.../src/graph_blocks/node_add.h | 17 +++++
.../src/graph_blocks/node_constant.c | 25 +++++++
.../src/graph_blocks/node_constant.h | 16 ++++
.../src/graph_blocks/node_gain.c | 25 +++++++
.../src/graph_blocks/node_gain.h | 20 +++++
.../src/graph_blocks/node_mult.c | 25 +++++++
.../src/graph_blocks/node_mult.h | 18 +++++
.../src/graph_blocks/node_pow.c | 26 +++++++
.../src/graph_blocks/node_pow.h | 20 +++++
.../modular_quad_pid/src/control_algorithm.c | 73 ++++++++++++++-----
.../modular_quad_pid/src/control_algorithm.h | 2 +-
.../src/graph_blocks/node_bounds.h | 2 +-
.../src/graph_blocks/node_constant.c | 26 +------
.../src/graph_blocks/node_constant.h | 17 +----
.../src/graph_blocks/node_mixer.c | 42 +++++++++++
.../src/graph_blocks/node_mixer.h | 23 ++++++
quad/sw/modular_quad_pid/src/type_def.h | 8 ++
21 files changed, 409 insertions(+), 60 deletions(-)
create mode 100644 quad/computation_graph/src/graph_blocks/node_accumulator.c
create mode 100644 quad/computation_graph/src/graph_blocks/node_accumulator.h
create mode 100644 quad/computation_graph/src/graph_blocks/node_add.c
create mode 100644 quad/computation_graph/src/graph_blocks/node_add.h
create mode 100644 quad/computation_graph/src/graph_blocks/node_constant.c
create mode 100644 quad/computation_graph/src/graph_blocks/node_constant.h
create mode 100644 quad/computation_graph/src/graph_blocks/node_gain.c
create mode 100644 quad/computation_graph/src/graph_blocks/node_gain.h
create mode 100644 quad/computation_graph/src/graph_blocks/node_mult.c
create mode 100644 quad/computation_graph/src/graph_blocks/node_mult.h
create mode 100644 quad/computation_graph/src/graph_blocks/node_pow.c
create mode 100644 quad/computation_graph/src/graph_blocks/node_pow.h
mode change 100644 => 120000 quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
mode change 100644 => 120000 quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
create mode 100644 quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c
create mode 100644 quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.h
diff --git a/quad/computation_graph/src/computation_graph.c b/quad/computation_graph/src/computation_graph.c
index c2f0df661..48c86467d 100644
--- a/quad/computation_graph/src/computation_graph.c
+++ b/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);
diff --git a/quad/computation_graph/src/graph_blocks/node_accumulator.c b/quad/computation_graph/src/graph_blocks/node_accumulator.c
new file mode 100644
index 000000000..17a36ee92
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_accumulator.c
@@ -0,0 +1,39 @@
+#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);
+}
diff --git a/quad/computation_graph/src/graph_blocks/node_accumulator.h b/quad/computation_graph/src/graph_blocks/node_accumulator.h
new file mode 100644
index 000000000..2df06c9d1
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_accumulator.h
@@ -0,0 +1,16 @@
+#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__
diff --git a/quad/computation_graph/src/graph_blocks/node_add.c b/quad/computation_graph/src/graph_blocks/node_add.c
new file mode 100644
index 000000000..a28ca657b
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_add.c
@@ -0,0 +1,25 @@
+#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);
+}
diff --git a/quad/computation_graph/src/graph_blocks/node_add.h b/quad/computation_graph/src/graph_blocks/node_add.h
new file mode 100644
index 000000000..cfd2b71e8
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_add.h
@@ -0,0 +1,17 @@
+#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__
diff --git a/quad/computation_graph/src/graph_blocks/node_constant.c b/quad/computation_graph/src/graph_blocks/node_constant.c
new file mode 100644
index 000000000..47ab571d5
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_constant.c
@@ -0,0 +1,25 @@
+#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);
+}
diff --git a/quad/computation_graph/src/graph_blocks/node_constant.h b/quad/computation_graph/src/graph_blocks/node_constant.h
new file mode 100644
index 000000000..91cd6d055
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_constant.h
@@ -0,0 +1,16 @@
+#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__
diff --git a/quad/computation_graph/src/graph_blocks/node_gain.c b/quad/computation_graph/src/graph_blocks/node_gain.c
new file mode 100644
index 000000000..f89c6f9cb
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_gain.c
@@ -0,0 +1,25 @@
+#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);
+}
diff --git a/quad/computation_graph/src/graph_blocks/node_gain.h b/quad/computation_graph/src/graph_blocks/node_gain.h
new file mode 100644
index 000000000..b26d3bfe3
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_gain.h
@@ -0,0 +1,20 @@
+#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__
diff --git a/quad/computation_graph/src/graph_blocks/node_mult.c b/quad/computation_graph/src/graph_blocks/node_mult.c
new file mode 100644
index 000000000..2696719fd
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_mult.c
@@ -0,0 +1,25 @@
+#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);
+}
diff --git a/quad/computation_graph/src/graph_blocks/node_mult.h b/quad/computation_graph/src/graph_blocks/node_mult.h
new file mode 100644
index 000000000..32d2d873d
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_mult.h
@@ -0,0 +1,18 @@
+#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__
diff --git a/quad/computation_graph/src/graph_blocks/node_pow.c b/quad/computation_graph/src/graph_blocks/node_pow.c
new file mode 100644
index 000000000..dbbc1d707
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_pow.c
@@ -0,0 +1,26 @@
+#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);
+}
diff --git a/quad/computation_graph/src/graph_blocks/node_pow.h b/quad/computation_graph/src/graph_blocks/node_pow.h
new file mode 100644
index 000000000..11bdd9173
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_pow.h
@@ -0,0 +1,20 @@
+#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__
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index 4def3b2fa..8a37ad01a 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/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
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.h b/quad/sw/modular_quad_pid/src/control_algorithm.h
index 463e0f512..8e8a15b79 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.h
+++ b/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);
/**
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
index 7c8b7e92c..516ebc28a 100644
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
+++ b/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 {
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
deleted file mode 100644
index 47ab571d5..000000000
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
+++ /dev/null
@@ -1,25 +0,0 @@
-#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);
-}
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
new file mode 120000
index 000000000..eb451afca
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
@@ -0,0 +1 @@
+/local/ucart/MicroCART_17-18/quad/computation_graph/src/graph_blocks/node_constant.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
deleted file mode 100644
index 91cd6d055..000000000
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#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__
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
new file mode 120000
index 000000000..a47be0f20
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
@@ -0,0 +1 @@
+/local/ucart/MicroCART_17-18/quad/computation_graph/src/graph_blocks/node_constant.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c
new file mode 100644
index 000000000..8b702b360
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c
@@ -0,0 +1,42 @@
+#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);
+}
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.h
new file mode 100644
index 000000000..1d58cf54d
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.h
@@ -0,0 +1,23 @@
+#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__
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 6bbe413f4..cdc34ce16 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/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;
/**
--
GitLab