main.cpp

00001 #include "math.h"
00002 #include "mbed.h"
00003 #include "rtos.h"
00004 #include "esc.h"
00005 #include "FreeIMU.h"
00006 #include <string>
00007 
00008 
00009 //Author: Matteo Terruzzi
00010 
00011 //Target differential of time in seconds, for the various program cycles:
00012 #define TARGET_IMU_DT   0.002  //500Hz   on main thread, every step of the cycle.
00013 #define TARGET_MAIN_DT  0.016  //62.5Hz  on main thread, every 8 steps + 7 step delay.
00014 #define TARGET_TX_DT    0.050  //20Hz    on other thread. Exchanged with main every 24 steps + 21 step delay (dt=48ms, 20.8333Hz)
00015 #define TARGET_RX_DT    0.050  //waited with previous: this dt is only measured, not waited.
00016 
00017 //For the various dt, "average" is computed using a sort of low-pass filter of the actual value.
00018 //The formula (recurrence equation) is:
00019 //    a(t) = x(t) * k - a(t-1) * k      with a(0) = 0
00020 //where:
00021 //    x(t)   is the actual value at the time t;
00022 //    a(t)   is the average at the timepoint t;
00023 //    a(t-1) is the average at the previous timepoint;
00024 //    k is AVERAGE_DT_K_GAIN (macro defined below).
00025 //
00026 //So, here is the meaning of AVERAGE_DT_K_GAIN, or k: the higher it is, the higher is the cut-frequency.
00027 //If k == 1, then a(t) == x(t).
00028 //If k == 0, then a(t) == target value (the actual value is totaly ignored).
00029 //A good value may be k = 1/8 = 0.125.
00030 #define AVERAGE_DT_K_GAIN 0.0125
00031 
00032 //Blinking status leds
00033 #define LED_IMU  LED1
00034 #define LED_ESC  LED2
00035 #define LED_TXRX LED3
00036 #define LED_RX   LED4
00037 
00038 #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); }
00039 #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); }
00040 #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);}
00041 
00042 #include "state.h"
00043 #include "TXRX_magic.h"
00044 
00045 FreeIMU IMU_imu;
00046     
00047 int main()
00048 {
00049     
00050 
00051     //INITIALIZATION
00052     //-------------------------------------------------------------------------------
00053     
00054     //Setup ESCs
00055     ESC esc1(p24); esc1=0;
00056     ESC esc2(p23); esc2=0;
00057     ESC esc3(p22); esc3=0;
00058     ESC esc4(p21); esc4=0;
00059     
00060     //Setup status leds
00061     DigitalOut  led_imu(LED_IMU);   led_imu = 0;
00062     PwmOut      led_esc(LED_ESC);   led_esc = 0;
00063     DigitalOut  led_txrx(LED_TXRX); led_txrx = 0;
00064     FAST_FLASH_ON(led_esc,5);
00065     
00066     //Setup remote control
00067     FAST_FLASH_OFF(led_txrx,2);
00068     Thread TXRX_thread(TXRX_thread_routine);
00069     Thread::wait(6000);
00070     FAST_FLASH_ON(led_txrx,5);
00071     
00072     //Setup IMU
00073     Thread::wait(2000);
00074     FAST_FLASH_OFF(led_imu,2);
00075     IMU_imu.init(true);
00076     Thread::wait(2000);
00077     FAST_FLASH_ON(led_imu,5);
00078     
00079     //Setup state
00080     QuadState mainQuadState;  mainQuadState.reset();
00081     QuadState rxQuadState;    rxQuadState.reset();
00082     QuadState txQuadState;    txQuadState.reset();
00083     
00084     
00085     
00086     //MAIN CONTROL CYCLE PREPARATION
00087     //-------------------------------------------------------------------------------
00088     
00089     unsigned short int step = 0;
00090     
00091     //Just a moment... Let the other threads begin!
00092     Thread::yield();
00093     Thread::wait((TARGET_MAIN_DT + TARGET_IMU_DT + TARGET_TX_DT + TARGET_RX_DT) * 10.0);
00094         //just made sure the other threads had time to begin.
00095         
00096     
00097     //Some flashing animation
00098     for(int i=100;i>10;i=(75*i)/100){
00099         led_txrx = 0;led_esc = 0;led_imu = 0; Thread::wait(i);
00100         led_txrx = 1;led_esc = 1;led_imu = 1; Thread::wait(i);
00101     }
00102     
00103     //Setup timers
00104     Timer       dt_timer;     dt_timer.reset();     dt_timer.start();
00105     Timer       IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
00106     Thread::wait(TARGET_IMU_DT);
00107     
00108     
00109    
00110     //MAIN CONTROL CYCLE
00111     //===================================================================================
00112     while(1)
00113     {
00114     
00115     
00116         //measure the time for IMU cycle
00117         Thread::wait(1);
00118         QUAD_STATE_READ_ACTUAL_DT(mainQuadState,imu,IMU_dt_timer)
00119         QUAD_STATE_WAIT_DT_TARGET(mainQuadState.actual_imu_dt,mainQuadState.target_imu_dt)
00120         QUAD_STATE_UPDATE_DT(mainQuadState,imu,IMU_dt_timer)
00121         
00122 
00123 if(step % 8 != 7){//see step%8==7; prevent double sensor refresh        
00124 
00125 
00126         //IMU INPUT
00127         //-------------------------------------------------------------------------------
00128 
00129         led_imu = 1;
00130         IMU_imu.getQ(NULL);
00131         led_imu = 0;
00132         
00133         
00134 }//end of 7/8 steps
00135 if(step % 8 == 7){
00136         
00137         
00138         //IMU INPUT AND ANGLES COMPUTATION
00139         //-------------------------------------------------------------------------------
00140         
00141         {
00142             float ypr[3];
00143             led_imu = 1;
00144             IMU_imu.getYawPitchRoll(ypr);
00145             mainQuadState.estimated_position_z = IMU_imu.getBaroAlt();
00146             IMU_imu.baro->getTemperature();
00147             mainQuadState.estimated_rotation_y = ypr[0]; //yaw = yaw
00148             mainQuadState.estimated_rotation_p = ypr[2]; //pitch = roll
00149             mainQuadState.estimated_rotation_r = ypr[1]; //roll = pitch
00150             led_imu = 0;
00151         }
00152         if(mainQuadState.actual_imu_dt >= 100.0 * mainQuadState.target_imu_dt)
00153             mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
00154     
00155     
00156         //ELABORATION
00157         //-------------------------------------------------------------------------------
00158         
00159         //Measure the time for main control cycle
00160         QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
00161         
00162         //Elaborate inputs to determinate outputs
00163         //led_esc = 1;
00164         
00165         {//Roll PID
00166             float previous = mainQuadState.pid_rotation_r_error;
00167             mainQuadState.pid_rotation_r_error = mainQuadState.target_rotation_r - mainQuadState.estimated_rotation_r;
00168             mainQuadState.pid_rotation_r_error_integrative += mainQuadState.pid_rotation_r_error;
00169             mainQuadState.pid_rotation_r_error_derivative = mainQuadState.pid_rotation_r_error - previous;
00170             mainQuadState.pid_rotation_r_out = mainQuadState.pid_rotation_r_kP * mainQuadState.pid_rotation_r_error
00171                                              + mainQuadState.pid_rotation_r_kI * mainQuadState.pid_rotation_r_error_integrative
00172                                              + mainQuadState.pid_rotation_r_kD * mainQuadState.pid_rotation_r_error_derivative;
00173             if(mainQuadState.pid_rotation_r_error_integrative > 100) mainQuadState.pid_rotation_r_error_integrative = 100;
00174             else if(mainQuadState.pid_rotation_r_error_integrative < -100) mainQuadState.pid_rotation_r_error_integrative = -100;
00175         }{//Pitch PID
00176             float previous = mainQuadState.pid_rotation_p_error;
00177             mainQuadState.pid_rotation_p_error = mainQuadState.target_rotation_p - mainQuadState.estimated_rotation_p;
00178             mainQuadState.pid_rotation_p_error_integrative += mainQuadState.pid_rotation_p_error;
00179             mainQuadState.pid_rotation_p_error_derivative = mainQuadState.pid_rotation_p_error - previous;
00180             mainQuadState.pid_rotation_p_out = mainQuadState.pid_rotation_p_kP * mainQuadState.pid_rotation_p_error
00181                                              + mainQuadState.pid_rotation_p_kI * mainQuadState.pid_rotation_p_error_integrative
00182                                              + mainQuadState.pid_rotation_p_kD * mainQuadState.pid_rotation_p_error_derivative;
00183             if(mainQuadState.pid_rotation_p_error_integrative > 100) mainQuadState.pid_rotation_p_error_integrative = 100;
00184             else if(mainQuadState.pid_rotation_p_error_integrative < -100) mainQuadState.pid_rotation_p_error_integrative = -100;
00185         }{//Yaw PID
00186             float previous = mainQuadState.pid_rotation_y_error;
00187             mainQuadState.pid_rotation_y_error = mainQuadState.target_rotation_y - mainQuadState.estimated_rotation_y;
00188             mainQuadState.pid_rotation_y_error_integrative += mainQuadState.pid_rotation_y_error;
00189             mainQuadState.pid_rotation_y_error_derivative = mainQuadState.pid_rotation_y_error - previous;
00190             mainQuadState.pid_rotation_y_out = mainQuadState.pid_rotation_y_kP * mainQuadState.pid_rotation_y_error
00191                                              + mainQuadState.pid_rotation_y_kI * mainQuadState.pid_rotation_y_error_integrative
00192                                              + mainQuadState.pid_rotation_y_kD * mainQuadState.pid_rotation_y_error_derivative;
00193             if(mainQuadState.pid_rotation_y_error_integrative > 100) mainQuadState.pid_rotation_y_error_integrative = 100;
00194             else if(mainQuadState.pid_rotation_y_error_integrative < -100) mainQuadState.pid_rotation_y_error_integrative = -100;
00195         }
00196         //Compute actual throttle values
00197         if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
00198           || rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){
00199             mainQuadState.actual_throttle_1 = 0;
00200             mainQuadState.actual_throttle_2 = 0;
00201             mainQuadState.actual_throttle_3 = 0;
00202             mainQuadState.actual_throttle_4 = 0;
00203         }else{
00204             mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
00205             mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
00206             mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
00207             mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
00208             
00209             mainQuadState.reference_throttle_1 = rxQuadState.reference_throttle_1;
00210             mainQuadState.reference_throttle_2 = rxQuadState.reference_throttle_2;
00211             mainQuadState.reference_throttle_3 = rxQuadState.reference_throttle_3;
00212             mainQuadState.reference_throttle_4 = rxQuadState.reference_throttle_4;
00213             
00214             if(mainQuadState.reference_throttle_1 <= -1000)
00215                 mainQuadState.actual_throttle_1 = 0;
00216             else{
00217                 float out = mainQuadState.reference_throttle_1; //NOTE: here we will have full throttle calcs..
00218                 out += mainQuadState.pid_rotation_r_out;
00219                 out += mainQuadState.pid_rotation_p_out;
00220                 out += mainQuadState.pid_rotation_y_out;
00221                 mainQuadState.actual_throttle_1 = out;
00222             }
00223             
00224             if(mainQuadState.reference_throttle_2 <= -1000)
00225                 mainQuadState.actual_throttle_2 = 0;
00226             else{
00227                 float out = mainQuadState.reference_throttle_2;
00228                 out += mainQuadState.pid_rotation_r_out;
00229                 out -= mainQuadState.pid_rotation_p_out;
00230                 out -= mainQuadState.pid_rotation_y_out;
00231                 mainQuadState.actual_throttle_2 = out;
00232             }
00233             
00234             if(mainQuadState.reference_throttle_3 <= -1000)
00235                 mainQuadState.actual_throttle_3 = 0;
00236             else{
00237                 float out = mainQuadState.reference_throttle_3;
00238                 out -= mainQuadState.pid_rotation_r_out;
00239                 out -= mainQuadState.pid_rotation_p_out;
00240                 out += mainQuadState.pid_rotation_y_out;
00241                 mainQuadState.actual_throttle_3 = out;
00242             }
00243             
00244             if(mainQuadState.reference_throttle_4 <= -1000)
00245                 mainQuadState.actual_throttle_4 = 0;
00246             else{
00247                 float out = mainQuadState.reference_throttle_4;
00248                 out -= mainQuadState.pid_rotation_r_out;
00249                 out += mainQuadState.pid_rotation_p_out;
00250                 out -= mainQuadState.pid_rotation_y_out;
00251                 mainQuadState.actual_throttle_4 = out;
00252             }
00253         }
00254         
00255         if      (mainQuadState.actual_throttle_1 > 1) mainQuadState.actual_throttle_1 = 1;
00256         else if (mainQuadState.actual_throttle_1 < 0) mainQuadState.actual_throttle_1 = 0;
00257         if      (mainQuadState.actual_throttle_2 > 1) mainQuadState.actual_throttle_2 = 1;
00258         else if (mainQuadState.actual_throttle_2 < 0) mainQuadState.actual_throttle_2 = 0;
00259         if      (mainQuadState.actual_throttle_3 > 1) mainQuadState.actual_throttle_3 = 1;
00260         else if (mainQuadState.actual_throttle_3 < 0) mainQuadState.actual_throttle_3 = 0;
00261         if      (mainQuadState.actual_throttle_4 > 1) mainQuadState.actual_throttle_4 = 1;
00262         else if (mainQuadState.actual_throttle_4 < 0) mainQuadState.actual_throttle_4 = 0;
00263         
00264         
00265         //OUTPUT
00266         //-------------------------------------------------------------------------------
00267         
00268         //set new ESC values.
00269         esc1 = mainQuadState.actual_throttle_1;
00270         esc2 = mainQuadState.actual_throttle_2;
00271         esc3 = mainQuadState.actual_throttle_3;
00272         esc4 = mainQuadState.actual_throttle_4;
00273         
00274         //effectively output to ESCs
00275         esc1();
00276         esc2();
00277         esc3();
00278         esc4();
00279         led_esc = mainQuadState.actual_throttle_1;
00280         
00281                
00282 } //end of 1/8 steps
00283 if(step % 24 == 21){
00284         
00285         
00286         //REMOTE INPUT/OUTPUT EXCHANGE
00287         //-------------------------------------------------------------------------------
00288         
00289         txQuadState = mainQuadState; //send info back
00290         txQuadState.timestamp = time(NULL);
00291         if(TXRX_stateExchange(txQuadState,rxQuadState)){
00292             
00293             led_txrx = 1;
00294             
00295             mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
00296             mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
00297             
00298             mainQuadState.pid_rotation_r_kP = rxQuadState.pid_rotation_r_kP;
00299             mainQuadState.pid_rotation_r_kI = rxQuadState.pid_rotation_r_kI;
00300             mainQuadState.pid_rotation_r_kD = rxQuadState.pid_rotation_r_kD;
00301             mainQuadState.pid_rotation_p_kP = rxQuadState.pid_rotation_p_kP;
00302             mainQuadState.pid_rotation_p_kI = rxQuadState.pid_rotation_p_kI;
00303             mainQuadState.pid_rotation_p_kD = rxQuadState.pid_rotation_p_kD;
00304             mainQuadState.pid_rotation_y_kP = rxQuadState.pid_rotation_y_kP;
00305             mainQuadState.pid_rotation_y_kI = rxQuadState.pid_rotation_y_kI;
00306             mainQuadState.pid_rotation_y_kD = rxQuadState.pid_rotation_y_kD;
00307             
00308             mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER;
00309             if(mainQuadState.actual_rx_dt >= 100.0 * mainQuadState.target_rx_dt)
00310                 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
00311         }
00312         led_txrx = 0;
00313         
00314         
00315 }//end of 1/24 steps
00316 
00317         
00318         ++step;
00319     }//End of main control loop
00320     
00321     
00322     //It must not get here.
00323     return 0;
00324 }