Quadcopter algorithm

control.c

#include <stdio.h>


typedef struct {
    /*
     * A struct used for storing the configured state of a Quadcopter's motors,
     * such as the maximmum power output per motor and the other tuning paramters
     * that effect how the quad will calculate the thrust ratios of the motors
     */

    // motor_max is a double, should be in range 0.0 - 100.0 and is the maximmum
    // allowed power that the motors should run at, as a percentage of the motor's
    // maximmum power that can be achieved from the software
    double motor_max;
    // pitch_max is a double specifying the bounds of the pitch control variable
    // this should typically be set to 180.0, giving +180.0 as being fully positive
    // and -180.0 as being fully negative.
    double pitch_max;
    // roll_max is identical in behaviour to pitch_max, but operating on the roll
    // control variable
    double roll_max;
    // while technically the same as pitch_max and roll_max, yaw_max is a bit
    // different in that yaw is not an absolute measurement, but rather a measurement
    // of angular change. so +180 would represent the intention to turn clockwise by 180 degrees
    double yaw_max;
    // motor_limit is a double, should be between 0.0 and motor_max and is used
    // to set a limit on how powerful the quad's motors should run.
    // This is different from motor_max, which tells the software what value to
    // consider as max power, whereas this parameter specifies a rule on which level
    // of power should not be exceeded during normal operation.
    double motor_limit;
    // pitch_limit is the maximumm allowable angle for the quadcopter to tilt in]
    // the positive and negative pitch directions (forwards and backwards).
    // This is provided to set a sensible hard limit on the angle that the software
    // will try to attain, to avoid potential acrobatic mishaps!
    // Tune these according to your requirements
    double pitch_limit;
    // roll_limit is same as pitch limit, for roll axis
    double roll_limit;
    // same as above, for yaw axis
    double yaw_limit;
    // master gain - controls how much the angular parameters are allowed to change the
    // final output of a motor, in proportion to the main thrust
    double master_gain;
    // angular gains - these are gain parameters that are applied to the input
    // angles of pitch, roll and yaw and are used to tune the copter's responsiveness
    // for the different axes
    double pitch_gain;
    double roll_gain;
    double yaw_gain;
} Quadcopter_Config;


typedef struct {
    /*
     * A struct used for representing the current state of the quadcopter,
     * mainly the desired power output of the four motors.
     * 
     * This is the lowest level of abstraction of the copter's state, these
     * values relate directly to the rotation speed of the motors, no complex
     * mathematical transforms are needed beyond here.
     */

    // front_left motor output
    double front_left;
    // front_right motor output
    double front_right;
    // back_left motor output
    double back_left;
    // back_right motor output
    double back_right;
} Quadcopter_State;


typedef struct {
    /*
     * This struct is used to store the state of the Quadcopter's control inputs.
     * 
     * These are four main variables, the angles of the pitch, roll and yaw and
     * The total power to apply across all four motors (total thrust for altitude
     * control).
     */

    // the amount of thrust to apply per motor, if the quadcopter were hovering
    // perfectly level.
    double thrust;
    double pitch; // pitch angle - 0.0 is neutral, max range defined in config
    double roll; // roll angle - same as above, for roll
    double yaw; // same as above, for yaw
} Quadcopter_Control_State;


double normalise(double input, double min_range, double max_range) {
    /*
     * Given an input value and the min and max ranges that this value can hold,
     * return an output value that is guaranteed to be either the input if it is
     * within the given range, or the closest of min or max if not.
     */

    // set output initially to value of input, in the hope that it is valid
    double output = input;
    if (output < min_range) {
        // too small, set to minimun
        output = min_range;
    } else if (output > max_range) {
        // too big, set to maximum
        output = max_range;
    } // otherwise, we're good to go with the value already
    return output;
}


Quadcopter_Control_State normalise_control_inputs(Quadcopter_Control_State input, Quadcopter_Config config) {
    /*
     * Given a struct representing control inputs from the controller, and a
     * struct containing this copter's config, normalise any erroneous control
     * values to be within the maximmum limits defined in this quadcopter's config,
     * returns the updated struct.
     */

    Quadcopter_Control_State output;
    // normalise input values
    output.thrust = normalise(input.thrust, 0.0, config.motor_limit);
    output.pitch = normalise(input.pitch, -config.pitch_limit, config.pitch_limit);
    output.roll = normalise(input.roll, -config.roll_limit, config.roll_limit);
    output.yaw = normalise(input.yaw, -config.yaw_limit, config.yaw_limit);
    return output;
}


Quadcopter_State calculate_power_ratios(Quadcopter_Control_State control, Quadcopter_Config config) {
    /*
     * Given a control state struct and a config struct, return a quadcopter state struct
     * with the different motor thrust values reflecting the state of the controls,
     * using an algorithm to work out how to divide up thrust ratios between different motors.
     */

    Quadcopter_State state;
    // calculate some secondary values that are used many times first
    // TODO: Move out into a function that calculates this once after init of config struct
    double gain = 2.0 / config.master_gain; // configures how much +/- of motor thrust to allow motors to become
    double pitch = config.pitch_gain * (control.pitch / config.pitch_limit);
    double roll = config.roll_gain * (control.roll / config.roll_limit);
    double yaw = config.yaw_gain * (control.yaw / config.yaw_limit);
    // calculate thrust output of each motor
    state.front_left = control.thrust * (1.0 + ((((-pitch + roll + -yaw) / 3.0) + 1.0) / gain));
    state.front_right = control.thrust * (1.0 + ((((-pitch + -roll + yaw) / 3.0) + 1.0) / gain));
    state.back_left = control.thrust * (1.0 + ((((pitch + roll + yaw) / 3.0) + 1.0) / gain));
    state.back_right = control.thrust * (1.0 + ((((pitch + -roll + -yaw) / 3.0) + 1.0) / gain));
    return state;
}


int main(int argc, char const *argv[]) {
    // construct and populate config struct
    Quadcopter_Config config;
    config.motor_max = 100.0;
    config.motor_limit = 85.0;
    config.pitch_max = 180.0;
    config.pitch_limit = 45.0;
    config.roll_max = 180.0;
    config.roll_limit = 45.0;
    config.yaw_max = 180.0;
    config.yaw_limit = 45.0;
    // willing to allow algo to output up to 50% more or less than reference thrust
    config.master_gain = 0.5;
    config.pitch_gain = 1.0;
    config.roll_gain = 1.0;
    config.yaw_gain = 1.0;

    // construct and populate control struct
    Quadcopter_Control_State control;
    control.thrust = 100.0;
    // values for leaning 15 deg forwards, rotating right by 5 deg
    control.pitch = 15.0;
    control.roll = 0.0;
    control.yaw = 0.0;

    // normalise inputs
    control = normalise_control_inputs(control, config);

    // calculate quad motor state
    Quadcopter_State state = calculate_power_ratios(control, config);

    printf(
        "FL:\t%lf\tFR:\t%lf\nBL:\t%lf\tBR:\t%lf\n",
        state.front_left, state.front_right,
        state.back_left, state.back_right
    );
    return 0;
}

quad_algo.txt

variable names:
    angular velocities:
        m = maximmum desired output for one prop
        fl = front-left prop
        fr = front-right prop
        bl = back-left prop
        br = back-right prop
    angle measurements:
        pm = max pitch angle (typically 180)
        p = desired pitch angle (in range pm -> -pm)
        rm = max roll angle (typically 180)
        r = desired roll angle (in range rm -> -rm)
        w = desired yaw angular change (in range wm -> -wm)
        wm = max yaw angular change (technically a tuning param as could be changed)
    angle tuning params:
        kp = coefficient of pitch (gain)
        kr = coefficient of roll (gain)
        kw = coefficient of yaw (gain)


algorithms:

fl = m * (((((kp * (p / pm)) + (kr * (r / -rm)) + (kw * (w / -wm))) / 3.0) + 1.0) / 2.0)

fr = m * (((((kp * (p / pm)) + (kr * (r / rm)) + (kw * (w / wm))) / 3.0) + 1.0) / 2.0)

bl = m * (((((kp * (p / -pm)) + (kr * (r / -rm)) + (kw * (w / wm))) / 3.0) + 1.0) / 2.0)

br = m * (((((kp * (p / -pm)) + (kr * (r / rm)) + (kw * (w / -wm))) / 3.0) + 1.0) / 2.0)