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:
2012-12-15
Revision:
29:8b7362a2ee14
Parent:
28:ba6ca9f4def4
Child:
30:021e13b62575

File content as of revision 29:8b7362a2ee14:

#include "Mixer.h"

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

void Mixer::compute(unsigned long dt, int Throttle, const float * controller_value)
{
    // Calculate new motorspeeds
    
    for(int i=0; i<4; i++)
        Motor_speed[i] = Throttle;
    
    Motor_speed[1] -= controller_value[0]; // Roll
    Motor_speed[3] += controller_value[0];
    
    Motor_speed[0] -= controller_value[1]; // Pitch
    Motor_speed[2] += controller_value[1];
    
    #if 0
        Motor_speed[1] -= controller_value[2]; // Yaw
        Motor_speed[3] -= controller_value[2];
        Motor_speed[0] += controller_value[2];
        Motor_speed[2] += controller_value[2];
    #endif
    
    for(int i = 0; i < 4; i++) // make shure no motor stands still
        Motor_speed[i] = Motor_speed[i] > 50 ? Motor_speed[i] : 50;
}