Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

Mixer/Mixer.cpp

Committer:
maetugr
Date:
2014-02-14
Revision:
40:2ca410923691
Parent:
38:ff95fd524c9e

File content as of revision 40:2ca410923691:

#include "Mixer.h"

Mixer::Mixer(int Configuration)
{
    Mixer::Configuration = Configuration;
    
    for(int i=0; i<4; i++)
        Motor_speed[i]=0;
}

void Mixer::compute(int Throttle, const float * controller_value)
{
    // Mixing tables for each configuration
    float mix_table[2][4][3] = {
    {
        {   0,  1,  1},       // + configuration
        {   1,  0, -1},
        {   0, -1,  1},
        {  -1,  0, -1}
    },
    {
        {  RT, -RT,  1},       // X configuration
        {  RT,  RT, -1},
        { -RT,  RT,  1},
        { -RT, -RT, -1}
    }
    };
    
    // Calculate new motorspeeds
    for(int i=0; i<4; i++) {
        Motor_speed[i] = Throttle;
        for(int j = 0; j < 3; j++)
            Motor_speed[i] += mix_table[Configuration][i][j] * controller_value[j];
    }
    
    for(int i = 0; i < 4; i++) { // make sure no motor stands still or gets a higher speed than 1000
        Motor_speed[i] = Motor_speed[i] > 70 ? Motor_speed[i] : 70;
        Motor_speed[i] = Motor_speed[i] <= 1000 ? Motor_speed[i] : 1000;
    }
}