Controller network

Introduction

The current structure of the control algorithm is a monolithic function that takes in all the inputs from the entire system, and outputs modified PWM values for the motors, which are then “mixed” to account for throttle and cross-coupling. This structure makes it difficult to dynamically change pieces of the controller to, for example, toggle between autonomous and manual flight, run the control algorithm from a remote computer, or disable controllers and inject values for testing. We propose a change to the structure of the control algorithm to separate the various pieces and allow for easy modification both in configuration and during runtime.

Structure

This proposal structures the control algorithm as a graph, with nodes having:

• Zero or more inputs
• Zero or more parameters
• Zero or more outputs

These nodes can be chained together, tying outputs to inputs of other nodes. Computation can then be initiated for one or more nodes, which will compute all necessary dependencies, and finally compute the requested node(s). The figure below shows an example computation graph.

Nodes can be defined by creating a corresponding .h and .c file.

An example header file for a block that adds two numbers:

#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__

And an example implementation for the add node:

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

Key components of the node definition

• Define the inputs, outputs, and parameters with unique names in the header, to make it intuitive when configuring the graph
• Create a concrete struct graph_node_type implementation, that describes general properties of this node type.
• Define an execute function. In this case, we called it add_nodes. This function takes in inputs and populates the output array. It is passed in as a function pointer when creating a node of this type.
• Optionally, create a state struct for keeping internal state of this node. The add block does not use this, but PID blocks, for example, do.
• Optionally, implement a reset function. This initializes the custom state. This example has an empty implementation, but NULL works as well.

Things to be aware of

Reset on connection

When a node is connected, via graph_set_source(destination, source), the source node will get its reset(state) function called if it was not connected to anything already. This resetting propagates up the dependency chain. See the Known Issues for more detail on how the propagation works.

Nodes only execute if their inputs have been updated

To allow for different sampling rates on PIDs, each node will only execute if one its ancestors has been updated. Updated in this sense means that graph_set_param() has been called, as that is the only way to get new values into the graph.

Known Issues

Reset propagation

Reset propagation only looks at direct children. When a node is reset, the reset is propagated up, and if the node’s only child is being reset, then it too will be reset. Maybe we should look at all descendents? It’s unclear whether all descendents should be considered unless it is already known which nodes “compute” will be called on. This diagram illustrates the problem. When D is first connected to B, B will be reset. Following that, A will be checked to see if it should be reset. Because A has another child, it will not be reset. Potentially, C is unused, in which case A should be reset. But it is unknown, so we only look at direct children currently.

Below is the entire control network currently implemented.