File content as of revision 4:46f3c3dd5977:

#include "math.h"
#include "mbed.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "esc.h"
#include "PID.h"
#include "MPU6050.h"
#include "IMUfilter.h"
#include <string>


//Author: Matteo Terruzzi

//Target differential of time in seconds, for the various program cycles:
                             //timings as for main cycle:
#define TARGET_IMU_DT  0.002 //every  1 step:  100%
#define TARGET_MAIN_DT 0.016 //every  8 steps: 12.5%  + 2 step start delay
#define TARGET_TX_DT   0.040 //every 20 steps: 5%     + 1 step start delay
#define TARGET_RX_DT   0.040 //with previous

//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_ESC LED1
#define LED_IMU LED2
#define LED_TXRX LED3
#define LED_RX LED4


#include "state.h"
#include "IMU.h"
#include "TXRX.h"

int main()
{

    Serial pc(USBTX,USBRX);
    

    //INITIALIZATION
    //-------------------------------------------------------------------------------
    
    //Setup ESCs
    ESC esc1(p23);
    ESC esc2(p24);
    ESC esc3(p25);
    ESC esc4(p26);
    
    //Setup status leds
    DigitalOut led_esc(LED_ESC); led_esc = 1;    //flip at esc update (fixed 0 means that the ESCs was not been initiated--the program is stucked before this point)
    DigitalOut led_imu(LED_IMU); led_imu = 0;    //flip at imu computations done (fixed 0 means that the imu sensors don't respond)
    DigitalOut led_txrx(LED_TXRX); led_txrx = 0; //flip at remote signal exchange done (fixed 0 means that the TXRX_thread has not been started)
    
    //Setup Ethernet
    EthernetInterface interface;
    interface.init("192.168.2.16","255.255.255.0","192.168.2.1");
    interface.connect();
    
    //Setup remote control
    Thread TXRX_thread (TXRX_thread_routine);
    led_txrx = 1;
    
    //Setup state
    QuadState mainQuadState;  mainQuadState.reset();
    QuadState rxQuadState;    rxQuadState.reset();
    QuadState txQuadState;    txQuadState.reset();
    
    //Setup FreeIMU
    IMUfilter    IMU_filter(TARGET_IMU_DT,40);
    MPU6050      IMU_sensor(p28, p27);
    IMU_sensor.setGyroRange(MPU6050_GYRO_RANGE_250);
    IMU_sensor.setAcceleroRange(MPU6050_ACCELERO_RANGE_4G);
    led_imu = 1;
    
    
    
    
    //MAIN CONTROL CYCLE PREPARATION
    //-------------------------------------------------------------------------------
    
    unsigned short int step = 0;
    
    //Just a moment... Let the other threads begin!
    Thread::wait(2000 /*ms*/);
    Thread::yield();
        //just made sure the other threads had time to begin.
        
    
    //Setup timers
    Timer   IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
    Timer       dt_timer;     dt_timer.reset();     dt_timer.start();
    Thread::wait(1000.0 * min(mainQuadState.target_imu_dt,mainQuadState.target_main_dt));
    
    //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);*/
   
   
    //MAIN CONTROL CYCLE
    //===================================================================================
    while(1)
    {
        
        
        //IMU INPUT
        //-------------------------------------------------------------------------------
        
        //Measure the time and sleep to correct the interval
        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)
        
        
        led_imu = 1;
        
        //Retrieve inertial values from sensors
        {
            float a[3], g[3];
            IMU_sensor.getAccelero(a);
            IMU_sensor.getGyro(g);
            
            mainQuadState.sensor_accel_x += a[0]/4 - mainQuadState.sensor_accel_x/4;
            mainQuadState.sensor_accel_y += a[1]/4 - mainQuadState.sensor_accel_y/4;
            mainQuadState.sensor_accel_z += a[2]/4 - mainQuadState.sensor_accel_z/4;
            mainQuadState.sensor_gyro_r = g[0];
            mainQuadState.sensor_gyro_p = g[1];
            mainQuadState.sensor_gyro_y = g[2];
            
        //Compute IMU filters
            IMU_filter.updateFilter(g[0], g[1], g[2],  a[0], a[1], a[2]);
        }
        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!!!
        
        led_imu = 0;
            
        
        
        
if(step % 20 == 2){
//5% frequency; step%20==2
        
        
        
        //REMOTE INPUT
        //-------------------------------------------------------------------------------
        led_txrx = 1;
        
        //Remote signal exchange
        txQuadState = mainQuadState; //send info back
        txQuadState.timestamp = time(NULL);
        if(TXRX_stateExchange(txQuadState,rxQuadState)){
            mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
            mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
            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!!!
        }
        
        
        /*if(step % 640 == 2){
        
            //Other tasks
            
            if(pc.writeable()){
                pc.printf("%s \r\n", mainQuadState.getJSON().c_str()); //the message is long => it slows down a lot!
            }
        }*/
        
        led_txrx = 0;
        

        
//end of 5% frequency; step%20==2
}
if(step % 8 == 3){
//12.5% frequency; step%8==3
    
    
        led_esc = 1;
    
    
        //ELABORATION
        //-------------------------------------------------------------------------------
            
        //Measure the time
        QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
        
        //Get estimated rotation
        //IMU_filter.computeEuler();
        //mainQuadState.estimated_rotation_y = IMU_filter.getYaw();
        //mainQuadState.estimated_rotation_p = IMU_filter.getPitch();
        //mainQuadState.estimated_rotation_r = IMU_filter.getRoll();
        IMU_filter.getCorrectedEuler(
                mainQuadState.estimated_rotation_r,
                mainQuadState.estimated_rotation_p,
                mainQuadState.estimated_rotation_y  );
        IMU_filter.getQuaternion(
                mainQuadState.estimated_rotation_q1,
                mainQuadState.estimated_rotation_q2,
                mainQuadState.estimated_rotation_q3,
                mainQuadState.estimated_rotation_q4);
        
        //Elaborate inputs to determinate outputs
        {//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;
        
            /*float roll = 0;
            if(mainQuadState.target_rotation_r_isRequired){
                targetRoll_pid.setSetPoint(mainQuadState.target_rotation_r);
                targetRoll_pid.setInterval(mainQuadState.average_main_dt);
                targetRoll_pid.setProcessValue(mainQuadState.estimated_rotation_r);
                roll = targetRoll_pid.compute();
            }else{
                targetRoll_pid.setSetPoint(mainQuadState.estimated_rotation_r);
            }*/
            
            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;
                mainQuadState.actual_throttle_1 = out;
            }
            
            if(mainQuadState.reference_throttle_2 <= -1000)
                mainQuadState.actual_throttle_2 = 0;
            else{
                float out = mainQuadState.reference_throttle_2;
                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;
                mainQuadState.actual_throttle_3 = out;
            }
            
            if(mainQuadState.reference_throttle_4 <= -1000)
                mainQuadState.actual_throttle_4 = 0;
            else{
                float out = mainQuadState.reference_throttle_4;
                mainQuadState.actual_throttle_4 = out;
            }
        }
        
        
        
        //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 = 0;
        
//end of 12.5% frequency; step%8==3
}
        
        
        
        ++step;
    }//End of main control loop
    
    //It must not get here.
    return 0;
}