File content as of revision 2:7439607ccd51:

#include "math.h"
#include "mbed.h"
#include "rtos.h"
//#include "EthernetInterface.h"
//#include "RefRX.h"
#include "esc.h"
#include "state.h"
#include "TXRX.h"
#include "IMU.h"
#include "PID.h"

int main()
{
    //Setup ESCs
    ESC esc1(p23);
    ESC esc2(p24);
    ESC esc3(p25);
    ESC esc4(p26);
    
    //Setup status leds
    DigitalOut led1(LED1); led1 = 1; //esc
    DigitalOut led4(LED4); led4 = 0; //signal
    //DigitalOut led3(LED3); led3 = 0; //tmp signal send
    //DigitalOut led2(LED2); led2 = 0;//moved to IMU.h
    
    //Setup Ethernet
    //EthernetInterface interface;
    //interface.init("192.168.1.16","255.255.255.0","192.168.1.1");
    //interface.connect();
    
    //Setup RefRX static class for remote control
    //Thread RefRX_thread (RefRX::worker);
    //RefRX::init(2222, 2223, RefRX_thread);
    //Serial pc(USBTX, USBRX);
    //pc.baud(115200);
    //pc.printf("Serial setted up\r\n");
    Thread TXRX_thread (TXRX_thread_routine);
    led4 = 1;
    //Thread::wait(0.5);
    
    //Setup FreeIMU
    //FreeIMU freeIMU;
    //freeIMU.init();
    Thread IMU_thread (IMU_thread_routine);
    //pc.printf("IMU thread started\r\n");
    
    //Setup state
    QuadState mainQuadState;  mainQuadState.reset();
    QuadState rxQuadState;    rxQuadState.reset();
    QuadState txQuadState;    txQuadState.reset();
    //pc.printf("Main and rx states setted up\r\n");
    
    //Prepare controll loop
    //float ref1=0, ref2=0, ref3=0, ref4=0;
    double dt=0.001; //will be measured
    Timer dt_t;
    dt_t.start();
    //unsigned int state_read_i=0;
    //unsigned int state_send_i=0;
    
    //pid
    PID targetRoll_pid(0.1,0.0001,0.5, 0.020);
    targetRoll_pid.setInputLimits(-3.15/4,3.15/4);
    targetRoll_pid.setOutputLimits(0,0.5);
    
    //pc.printf("Now going to start main loop...\r\n");
    
    led1 = !led1;
    //Begins main control loop
    while(1)
    {
        //retrive refs
        //led4 ^= RefRX::get(ref1,ref2,ref3,ref4); //flip-flop-flip-flop!
        /*
        if(pc.readable()){
            *((unsigned char *)(&rxQuadState) + state_read_i) = pc.getc();
            ++state_read_i;
            if(state_read_i >= sizeof(QuadState))
                state_read_i = 0;
            led4 = 1;
        }
        else
            led4 = 0;
        */
        unsigned int rx_delta, rx_wait;
        if(TXRX_stateExchange(txQuadState,rxQuadState))
            if(rxQuadState.actual_rx_delta_ms >= 5000 || rxQuadState.actual_rx_wait_ms >= 5000)
                mainQuadState.ZERO_ALL_MOTORS_NOW__DANGER = true; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
        
        //retrive inertial values
        IMU_filter_getYawPitchRoll(
            mainQuadState.estimated_rotation_y,
            mainQuadState.estimated_rotation_p,
            mainQuadState.estimated_rotation_r );
    
        //calc dt
        dt_t.stop();
        dt = (double) (1.0/1000000.0) * dt_t.read_us();
        dt_t.reset(); dt_t.start();
        mainQuadState.average_throttle_dt += dt * mainQuadState.average_throttle_dt_k;
        mainQuadState.average_throttle_dt -= mainQuadState.average_throttle_dt * mainQuadState.average_throttle_dt_k;
        mainQuadState.actual_throttle_dt = dt;
        
        //elaborate signals
        if(mainQuadState.ZERO_ALL_MOTORS_NOW__DANGER){
            mainQuadState.actual_throttle_1 = 0;
            mainQuadState.actual_throttle_2 = 0;
            mainQuadState.actual_throttle_3 = 0;
            mainQuadState.actual_throttle_4 = 0;
        }else{
            mainQuadState.actual_rx_delta_ms = rxQuadState.actual_rx_delta_ms;
            mainQuadState.actual_rx_wait_ms = rxQuadState.actual_rx_wait_ms;
            mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
            mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
            
        
            float roll = 0;
            if(mainQuadState.target_rotation_r_isRequired){
                targetRoll_pid.setSetPoint(mainQuadState.target_rotation_r);
                targetRoll_pid.setInterval(mainQuadState.average_throttle_dt);
                targetRoll_pid.setProcessValue(mainQuadState.estimated_rotation_r);
                roll = targetRoll_pid.compute();
            }else{
                targetRoll_pid.setSetPoint(mainQuadState.estimated_rotation_r);
            }
            
            if(rxQuadState.requested_throttle_1 >= 0 && rxQuadState.requested_throttle_1 <=1)
                mainQuadState.actual_throttle_1 = rxQuadState.requested_throttle_1;
            else{
                float out = 0.5; //NOTE: here we will have full throttle calcs..
                out -= roll;
                mainQuadState.actual_throttle_1 = out;
            }
            if(rxQuadState.requested_throttle_2 >= 0 && rxQuadState.requested_throttle_2 <=1)
                mainQuadState.actual_throttle_2 = rxQuadState.requested_throttle_2;
            else{
                mainQuadState.actual_throttle_2 = 0;
            }
            if(rxQuadState.requested_throttle_3 >= 0 && rxQuadState.requested_throttle_3 <=1)
                mainQuadState.actual_throttle_3 = rxQuadState.requested_throttle_3;
            else{
                float out = 0.5;
                out += roll;
                mainQuadState.actual_throttle_3 = out;
            }
            if(rxQuadState.requested_throttle_4 >= 0 && rxQuadState.requested_throttle_4 <=1)
                mainQuadState.actual_throttle_4 = rxQuadState.requested_throttle_4;
            else{
                mainQuadState.actual_throttle_4 = 0;
            }
        }
        
        //compute new ESC values..
        esc1 = mainQuadState.actual_throttle_1;
        esc2 = mainQuadState.actual_throttle_2;
        esc3 = mainQuadState.actual_throttle_3;
        esc4 = mainQuadState.actual_throttle_4;
        
        //finally output to ESCs
        esc1();
        esc2();
        esc3();
        esc4();
        
        led1 = !led1;
        
        
        uint32_t w = 20-dt;
        if(w<5) w=5;
        else if(w>19) w=19;
        Thread::wait(w /*ms*/);
        
        
        //send info back
        txQuadState = mainQuadState;
        
        /*
        for(int i=0; i<sizeof(QuadState); ++i){
            while(!pc.writeable())
                led3=0;
            pc.putc((int) *((unsigned char *) (&mainQuadState) + i));
        }*/
        /*
        if(state_send_i >= 7){
            state_send_i = 0;
            time_t sec = time(NULL);
            pc.printf("\033[2J\033[1;1H");
            pc.printf("Time: %s", ctime(&sec));
            pc.printf("Actual thrust: \t\t %8.2f  %8.2f  %8.2f  %8.2f\n",
                    mainQuadState.actual_throttle_1, mainQuadState.actual_throttle_2,
                    mainQuadState.actual_throttle_3, mainQuadState.actual_throttle_4 );
            pc.printf("IMU yaw pitch roll: \t %10.1f° %10.1f° %10.1f°\n", mainQuadState.estimated_rotation_y*180.0/3.141592653589793,
                    mainQuadState.estimated_rotation_p*180.0/3.141592653589793, mainQuadState.estimated_rotation_r*180.0/3.141592653589793 );
            pc.printf("Avg/actual delta time: \t %10.2f ms  %10.2f ms  \n", mainQuadState.average_throttle_dt*1000.0, mainQuadState.actual_throttle_dt*1000.0);
            led3=pc.writeable();
        }
        ++state_send_i;*/
    }
    
    return 0;
}