File content as of revision 7:cda17cffec3c:

#include "math.h"
#include "mbed.h"
#include "rtos.h"
#include "esc.h"
#include "FreeIMU.h"
#include <string>


//Author: Matteo Terruzzi

//Target differential of time in seconds, for the various program cycles:
#define TARGET_IMU_DT   0.002  //500Hz   on main thread, every step of the cycle.
#define TARGET_MAIN_DT  0.016  //62.5Hz  on main thread, every 8 steps + 7 step delay.
#define TARGET_TX_DT    0.050  //20Hz    on other thread. Exchanged with main every 24 steps + 21 step delay (dt=48ms, 20.8333Hz)
#define TARGET_RX_DT    0.050  //waited with previous: this dt is only measured, not waited.

//For the various dt, "average" is computed using a sort of low-pass filter of the actual value.
//The formula (recurrence equation) is:
//    a(t) = x(t) * k - a(t-1) * k      with a(0) = 0
//where:
//    x(t)   is the actual value at the time t;
//    a(t)   is the average at the timepoint t;
//    a(t-1) is the average at the previous timepoint;
//    k is AVERAGE_DT_K_GAIN (macro defined below).
//
//So, here is the meaning of AVERAGE_DT_K_GAIN, or k: the higher it is, the higher is the cut-frequency.
//If k == 1, then a(t) == x(t).
//If k == 0, then a(t) == target value (the actual value is totaly ignored).
//A good value may be k = 1/8 = 0.125.
#define AVERAGE_DT_K_GAIN 0.0125

//Blinking status leds
#define LED_IMU  LED1
#define LED_ESC  LED2
#define LED_TXRX LED3
#define LED_RX   LED4

#define FAST_FLASH_ON(led,times)  for(int i=0;i<times;++i){Thread::wait(25);  led = 0; Thread::wait(50);  led = 1; Thread::wait(25); }
#define FAST_FLASH_OFF(led,times) for(int i=0;i<times;++i){Thread::wait(25);  led = 1; Thread::wait(50);  led = 0; Thread::wait(25); }
#define SLOW_FLASH_ON(led,times)  for(int i=0;i<times;++i){Thread::wait(250); led = 0; Thread::wait(500); led = 1; Thread::wait(250);}

#include "state.h"
#include "TXRX_magic.h"

FreeIMU IMU_imu;
    
int main()
{
    

    //INITIALIZATION
    //-------------------------------------------------------------------------------
    
    //Setup ESCs
    ESC esc1(p24); esc1=0;
    ESC esc2(p23); esc2=0;
    ESC esc3(p22); esc3=0;
    ESC esc4(p21); esc4=0;
    
    //Setup status leds
    DigitalOut  led_imu(LED_IMU);   led_imu = 0;
    PwmOut      led_esc(LED_ESC);   led_esc = 0;
    DigitalOut  led_txrx(LED_TXRX); led_txrx = 0;
    FAST_FLASH_ON(led_esc,5);
    
    //Setup remote control
    FAST_FLASH_OFF(led_txrx,2);
    Thread TXRX_thread(TXRX_thread_routine);
    Thread::wait(6000);
    FAST_FLASH_ON(led_txrx,5);
    
    //Setup IMU
    Thread::wait(2000);
    FAST_FLASH_OFF(led_imu,2);
    IMU_imu.init(true);
    Thread::wait(2000);
    FAST_FLASH_ON(led_imu,5);
    
    //Setup state
    QuadState mainQuadState;  mainQuadState.reset();
    QuadState rxQuadState;    rxQuadState.reset();
    QuadState txQuadState;    txQuadState.reset();
    
    
    
    //MAIN CONTROL CYCLE PREPARATION
    //-------------------------------------------------------------------------------
    
    unsigned short int step = 0;
    
    //Just a moment... Let the other threads begin!
    Thread::yield();
    Thread::wait((TARGET_MAIN_DT + TARGET_IMU_DT + TARGET_TX_DT + TARGET_RX_DT) * 10.0);
        //just made sure the other threads had time to begin.
        
    
    //Some flashing animation
    for(int i=100;i>10;i=(75*i)/100){
        led_txrx = 0;led_esc = 0;led_imu = 0; Thread::wait(i);
        led_txrx = 1;led_esc = 1;led_imu = 1; Thread::wait(i);
    }
    
    //Setup timers
    Timer       dt_timer;     dt_timer.reset();     dt_timer.start();
    Timer       IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
    Thread::wait(TARGET_IMU_DT);
    
    
   
    //MAIN CONTROL CYCLE
    //===================================================================================
    while(1)
    {
    
    
        //measure the time for IMU cycle
        Thread::wait(1);
        QUAD_STATE_READ_ACTUAL_DT(mainQuadState,imu,IMU_dt_timer)
        QUAD_STATE_WAIT_DT_TARGET(mainQuadState.actual_imu_dt,mainQuadState.target_imu_dt)
        QUAD_STATE_UPDATE_DT(mainQuadState,imu,IMU_dt_timer)
        

if(step % 8 != 7){//see step%8==7; prevent double sensor refresh        


        //IMU INPUT
        //-------------------------------------------------------------------------------

        led_imu = 1;
        IMU_imu.getQ(NULL);
        led_imu = 0;
        
        
}//end of 7/8 steps
if(step % 8 == 7){
        
        
        //IMU INPUT AND ANGLES COMPUTATION
        //-------------------------------------------------------------------------------
        
        {
            float ypr[3];
            led_imu = 1;
            IMU_imu.getYawPitchRoll(ypr);
            mainQuadState.estimated_position_z = IMU_imu.getBaroAlt();
            IMU_imu.baro->getTemperature();
            mainQuadState.estimated_rotation_y = ypr[0]; //yaw = yaw
            mainQuadState.estimated_rotation_p = ypr[2]; //pitch = roll
            mainQuadState.estimated_rotation_r = ypr[1]; //roll = pitch
            led_imu = 0;
        }
        if(mainQuadState.actual_imu_dt >= 100.0 * mainQuadState.target_imu_dt)
            mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
    
    
        //ELABORATION
        //-------------------------------------------------------------------------------
        
        //Measure the time for main control cycle
        QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
        
        //Elaborate inputs to determinate outputs
        //led_esc = 1;
        
        {//Roll PID
            float previous = mainQuadState.pid_rotation_r_error;
            mainQuadState.pid_rotation_r_error = mainQuadState.target_rotation_r - mainQuadState.estimated_rotation_r;
            mainQuadState.pid_rotation_r_error_integrative += mainQuadState.pid_rotation_r_error;
            mainQuadState.pid_rotation_r_error_derivative = mainQuadState.pid_rotation_r_error - previous;
            mainQuadState.pid_rotation_r_out = mainQuadState.pid_rotation_r_kP * mainQuadState.pid_rotation_r_error
                                             + mainQuadState.pid_rotation_r_kI * mainQuadState.pid_rotation_r_error_integrative
                                             + mainQuadState.pid_rotation_r_kD * mainQuadState.pid_rotation_r_error_derivative;
            if(mainQuadState.pid_rotation_r_error_integrative > 100) mainQuadState.pid_rotation_r_error_integrative = 100;
            else if(mainQuadState.pid_rotation_r_error_integrative < -100) mainQuadState.pid_rotation_r_error_integrative = -100;
        }{//Pitch PID
            float previous = mainQuadState.pid_rotation_p_error;
            mainQuadState.pid_rotation_p_error = mainQuadState.target_rotation_p - mainQuadState.estimated_rotation_p;
            mainQuadState.pid_rotation_p_error_integrative += mainQuadState.pid_rotation_p_error;
            mainQuadState.pid_rotation_p_error_derivative = mainQuadState.pid_rotation_p_error - previous;
            mainQuadState.pid_rotation_p_out = mainQuadState.pid_rotation_p_kP * mainQuadState.pid_rotation_p_error
                                             + mainQuadState.pid_rotation_p_kI * mainQuadState.pid_rotation_p_error_integrative
                                             + mainQuadState.pid_rotation_p_kD * mainQuadState.pid_rotation_p_error_derivative;
            if(mainQuadState.pid_rotation_p_error_integrative > 100) mainQuadState.pid_rotation_p_error_integrative = 100;
            else if(mainQuadState.pid_rotation_p_error_integrative < -100) mainQuadState.pid_rotation_p_error_integrative = -100;
        }{//Yaw PID
            float previous = mainQuadState.pid_rotation_y_error;
            mainQuadState.pid_rotation_y_error = mainQuadState.target_rotation_y - mainQuadState.estimated_rotation_y;
            mainQuadState.pid_rotation_y_error_integrative += mainQuadState.pid_rotation_y_error;
            mainQuadState.pid_rotation_y_error_derivative = mainQuadState.pid_rotation_y_error - previous;
            mainQuadState.pid_rotation_y_out = mainQuadState.pid_rotation_y_kP * mainQuadState.pid_rotation_y_error
                                             + mainQuadState.pid_rotation_y_kI * mainQuadState.pid_rotation_y_error_integrative
                                             + mainQuadState.pid_rotation_y_kD * mainQuadState.pid_rotation_y_error_derivative;
            if(mainQuadState.pid_rotation_y_error_integrative > 100) mainQuadState.pid_rotation_y_error_integrative = 100;
            else if(mainQuadState.pid_rotation_y_error_integrative < -100) mainQuadState.pid_rotation_y_error_integrative = -100;
        }
        //Compute actual throttle values
        if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
          || rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){
            mainQuadState.actual_throttle_1 = 0;
            mainQuadState.actual_throttle_2 = 0;
            mainQuadState.actual_throttle_3 = 0;
            mainQuadState.actual_throttle_4 = 0;
        }else{
            mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
            mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
            mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
            mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
            
            mainQuadState.reference_throttle_1 = rxQuadState.reference_throttle_1;
            mainQuadState.reference_throttle_2 = rxQuadState.reference_throttle_2;
            mainQuadState.reference_throttle_3 = rxQuadState.reference_throttle_3;
            mainQuadState.reference_throttle_4 = rxQuadState.reference_throttle_4;
            
            if(mainQuadState.reference_throttle_1 <= -1000)
                mainQuadState.actual_throttle_1 = 0;
            else{
                float out = mainQuadState.reference_throttle_1; //NOTE: here we will have full throttle calcs..
                out += mainQuadState.pid_rotation_r_out;
                out += mainQuadState.pid_rotation_p_out;
                out += mainQuadState.pid_rotation_y_out;
                mainQuadState.actual_throttle_1 = out;
            }
            
            if(mainQuadState.reference_throttle_2 <= -1000)
                mainQuadState.actual_throttle_2 = 0;
            else{
                float out = mainQuadState.reference_throttle_2;
                out += mainQuadState.pid_rotation_r_out;
                out -= mainQuadState.pid_rotation_p_out;
                out -= mainQuadState.pid_rotation_y_out;
                mainQuadState.actual_throttle_2 = out;
            }
            
            if(mainQuadState.reference_throttle_3 <= -1000)
                mainQuadState.actual_throttle_3 = 0;
            else{
                float out = mainQuadState.reference_throttle_3;
                out -= mainQuadState.pid_rotation_r_out;
                out -= mainQuadState.pid_rotation_p_out;
                out += mainQuadState.pid_rotation_y_out;
                mainQuadState.actual_throttle_3 = out;
            }
            
            if(mainQuadState.reference_throttle_4 <= -1000)
                mainQuadState.actual_throttle_4 = 0;
            else{
                float out = mainQuadState.reference_throttle_4;
                out -= mainQuadState.pid_rotation_r_out;
                out += mainQuadState.pid_rotation_p_out;
                out -= mainQuadState.pid_rotation_y_out;
                mainQuadState.actual_throttle_4 = out;
            }
        }
        
        if      (mainQuadState.actual_throttle_1 > 1) mainQuadState.actual_throttle_1 = 1;
        else if (mainQuadState.actual_throttle_1 < 0) mainQuadState.actual_throttle_1 = 0;
        if      (mainQuadState.actual_throttle_2 > 1) mainQuadState.actual_throttle_2 = 1;
        else if (mainQuadState.actual_throttle_2 < 0) mainQuadState.actual_throttle_2 = 0;
        if      (mainQuadState.actual_throttle_3 > 1) mainQuadState.actual_throttle_3 = 1;
        else if (mainQuadState.actual_throttle_3 < 0) mainQuadState.actual_throttle_3 = 0;
        if      (mainQuadState.actual_throttle_4 > 1) mainQuadState.actual_throttle_4 = 1;
        else if (mainQuadState.actual_throttle_4 < 0) mainQuadState.actual_throttle_4 = 0;
        
        
        //OUTPUT
        //-------------------------------------------------------------------------------
        
        //set new ESC values.
        esc1 = mainQuadState.actual_throttle_1;
        esc2 = mainQuadState.actual_throttle_2;
        esc3 = mainQuadState.actual_throttle_3;
        esc4 = mainQuadState.actual_throttle_4;
        
        //effectively output to ESCs
        esc1();
        esc2();
        esc3();
        esc4();
        led_esc = mainQuadState.actual_throttle_1;
        
               
} //end of 1/8 steps
if(step % 24 == 21){
        
        
        //REMOTE INPUT/OUTPUT EXCHANGE
        //-------------------------------------------------------------------------------
        
        txQuadState = mainQuadState; //send info back
        txQuadState.timestamp = time(NULL);
        if(TXRX_stateExchange(txQuadState,rxQuadState)){
            
            led_txrx = 1;
            
            mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
            mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
            
            mainQuadState.pid_rotation_r_kP = rxQuadState.pid_rotation_r_kP;
            mainQuadState.pid_rotation_r_kI = rxQuadState.pid_rotation_r_kI;
            mainQuadState.pid_rotation_r_kD = rxQuadState.pid_rotation_r_kD;
            mainQuadState.pid_rotation_p_kP = rxQuadState.pid_rotation_p_kP;
            mainQuadState.pid_rotation_p_kI = rxQuadState.pid_rotation_p_kI;
            mainQuadState.pid_rotation_p_kD = rxQuadState.pid_rotation_p_kD;
            mainQuadState.pid_rotation_y_kP = rxQuadState.pid_rotation_y_kP;
            mainQuadState.pid_rotation_y_kI = rxQuadState.pid_rotation_y_kI;
            mainQuadState.pid_rotation_y_kD = rxQuadState.pid_rotation_y_kD;
            
            mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER;
            if(mainQuadState.actual_rx_dt >= 100.0 * mainQuadState.target_rx_dt)
                mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
        }
        led_txrx = 0;
        
        
}//end of 1/24 steps

        
        ++step;
    }//End of main control loop
    
    
    //It must not get here.
    return 0;
}