File content as of revision 1:ac68f0368a77:

/**
 * @author Erik van de Coevering
 *
 * @section LICENSE
 *
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge and/or publish copies of the Software, and to permit
 * persons to whom the Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "mbed.h"
#include "ADXL345.h"
#include "ITG3200.h"
#include "math.h"
//#include "accelero.h"
#include "bma180.h"
#include "gyro.h"
#include "IOMacros.h"
#include "pwmout.h"

DigitalOut myled(LED1);

Timer trudder;
Timer tthrottle;
Timer televator;
Timer taileron;

//Rx variables
int ruddertime = 1616;
int throttletime = 1100;
int elevatortime = 850;
int ailerontime = 850;
int ruddercenter = 1616;
int elevcenter = 1616;
int aileroncenter = 1616;

//Variables for calccomp
float xcomp = 0;
float ycomp = 0;
int xcomp2 = 0;
int ycomp2 = 0;
float ruddercomp = 0;
float ratio = 0.38;
int ailer = 0;
int elev = 0;
int rud = 0;
int zcomp = 0;
int throttle = 0;
float ailer2 = 0;
float elev2 = 0;
signed int m1 = 0;
signed int m2 = 0;
signed int m3 = 0;
signed int m4 = 0;
bool armed = false;
float xsteering = 0;
float ysteering = 0;

DigitalIn rxthrottle(p25);
DigitalIn rxaileron(p26);
DigitalIn rxelevator(p29);
DigitalIn rxrudder(p30);
DigitalIn rxextra(p19);

void calccomp(void)
{
    gyrosample();

    //Scale rx channels
    ailer = ailerontime - aileroncenter;
    elev = elevatortime - elevcenter;
    rud = ruddertime - ruddercenter;
    throttle = throttletime;

    xsteering = ailer;
    ysteering = elev;
    xsteering = xsteering/8;
    ysteering = ysteering/8;
    rud = rud/3;
    throttle = ((throttle/2.2)+720);

    //Start by mixing throttle
    m1 = throttle;
    m2 = throttle;
    m3 = throttle;
    m4 = throttle;

    //Calc aileron compensation and mix with rx input
    xcomp = (((((newax)-xsteering)*(1-ratio)) + ((yag+((newax-xsteering)*0.7))*ratio))*1.10);
    xcomp2 = (xcomp*-1);
 //   xcomp2 = 0;

    //Mix aileron
    m1 = m1 - xcomp2;
    m2 = m2 + xcomp2;
    m3 = m3 + xcomp2;
    m4 = m4 - xcomp2;

    //Calc elevator compensation and mix with rx input
    ycomp = (((((neway)+ysteering)*(1-ratio)) + ((xag+((neway+ysteering)*0.7))*ratio))*1.10);
    ycomp2 = ycomp;
   // ycomp2 = 0;

    //Mix elevator
    m1 = m1 - ycomp2;
    m2 = m2 - ycomp2;
    m3 = m3 + ycomp2;
    m4 = m4 + ycomp2;

    //Calc rudder compensation and mix with rx input -> PI-controller?
    ruddercomp = (rud + (zarg*1));  //has drift
    zcomp = ruddercomp;

    //Mix rudder
    m1 = m1 - zcomp;
    m2 = m2 + zcomp;
    m3 = m3 - zcomp;
    m4 = m4 + zcomp;

    //Prevent motors from stalling
//           if(m1 < 1075 && throttle > 1000) m1 = 1280;
//           if(m2 < 1075 && throttle > 1000) m2 = 1280;
//           if(m3 < 1075 && throttle > 1000) m3 = 1280;
//           if(m4 < 1075 && throttle > 1000) m4 = 1280;

    //When throttle down or if not armed, stop motors
    if(throttle < 1350 || armed == false) {
        m1=1200;
        m2=1200;
        m3=1200;
        m4=1200;
    }

    //Stick arming
    if(throttle < 1325 && rud > 120) {
        armed = true;
        myled = 1;
    }
    if(throttle < 1325 && rud < -105) {
        armed = false;
        myled = 0;
    }

    m3 += 20;

    if(m1 > 1640) m1 = 1640;
    if(m2 > 1640) m2 = 1640;
    if(m3 > 1640) m3 = 1640;
    if(m4 > 1640) m4 = 1640;

    //Output to motors
    setpwm(m1, m2, m3, m4);
}



int main()
{
    bool rudderflag = false;
    bool throttleflag = false;
    bool elevatorflag = false;
    bool aileronflag = false;
    bool extraflag = false;
    bool exitflag = true;

    int th = 0;
    int ai = 0;
    int el = 0;
    int ru = 0;

    myled = 0;

    startpwm();
    //  accstart();
    acc1.init();
    acc1.range(0);
    acc1.bw(7);
    gyrostart();
    wait(1);
    gyrocal();

    while(1) {
        if(!throttleflag && rxthrottle) {
            tthrottle.start();
            throttleflag = true;
            extraflag = false;
            calccomp();
        }

        if(!aileronflag && rxaileron) {
            th = tthrottle.read_us();
            if(th > 1000 && th < 2200) throttletime = th;
            taileron.start();
            tthrottle.reset();
            aileronflag = true;
            throttleflag = false;
            calccomp();
        }

        if(!elevatorflag && rxelevator) {
            ai = taileron.read_us();
            if(ai > 1000 && ai < 2200) ailerontime = ai;
            televator.start();
            taileron.reset();
            elevatorflag = true;
            aileronflag = false;
            calccomp();
        }

        if(!rudderflag && rxrudder) {
            el = televator.read_us();
            if(el > 1000 && el < 2200) elevatortime = el;
            trudder.start();
            televator.reset();
            rudderflag = true;
            elevatorflag = false;
            calccomp();
        }

        if(!extraflag && rxextra) {
            ru = trudder.read_us();
            if(ru > 1000 && ru < 2200) ruddertime = ru;
            trudder.reset();
            extraflag = true;
            rudderflag = false;
            calccomp();
            wait_us(500);
            calccomp();
            wait_us(500);
            calccomp();
            wait_us(500);
            calccomp();
            wait_us(500);
            calccomp();
            wait_us(500);
            calccomp();
            wait_us(500);
            calccomp();
            wait_us(500);
            calccomp();
            wait_us(500);
            calccomp();
        }
    }
}