Diff: main.cpp

Revision:

6:4ddd68260f76

Parent:

5:33abcc31b0aa

Child:

7:cda17cffec3c

diff -r 33abcc31b0aa -r 4ddd68260f76 main.cpp
--- a/main.cpp	Mon May 12 22:58:06 2014 +0000
+++ b/main.cpp	Tue May 13 22:56:44 2014 +0000
@@ -40,7 +40,7 @@
 #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_nomagic.h"
+#include "TXRX_magic.h"
 
 FreeIMU IMU_imu;
     
@@ -66,14 +66,14 @@
     //Setup remote control
     FAST_FLASH_OFF(led_txrx,2);
     Thread TXRX_thread(TXRX_thread_routine);
-    Thread::wait(8000);
+    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(8000);
+    Thread::wait(2000);
     FAST_FLASH_ON(led_imu,5);
     
     //Setup state
@@ -90,22 +90,21 @@
     
     //Just a moment... Let the other threads begin!
     Thread::yield();
-    Thread::wait(1000.0 * (TARGET_MAIN_DT + TARGET_IMU_DT + TARGET_TX_DT + TARGET_RX_DT) * 10.0);
+    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.
         
     
-    //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(1000.0 * TARGET_IMU_DT);
-    
-    
     //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
@@ -115,10 +114,10 @@
     
     
         //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)
-        wait_us(1000);
         
 
 if(step % 8 != 7){//see step%8==7; prevent double sensor refresh        
@@ -145,9 +144,9 @@
             IMU_imu.getYawPitchRoll(ypr);
             mainQuadState.estimated_position_z = IMU_imu.getBaroAlt();
             IMU_imu.baro->getTemperature();
-            mainQuadState.estimated_rotation_y = ypr[0];
-            mainQuadState.estimated_rotation_p = ypr[1];
-            mainQuadState.estimated_rotation_r = ypr[2];
+            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)
@@ -216,7 +215,9 @@
                 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_r_out;
+                out += mainQuadState.pid_rotation_p_out;
+                out += mainQuadState.pid_rotation_y_out;
                 mainQuadState.actual_throttle_1 = out;
             }
             
@@ -224,6 +225,9 @@
                 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;
             }
             
@@ -231,7 +235,9 @@
                 mainQuadState.actual_throttle_3 = 0;
             else{
                 float out = mainQuadState.reference_throttle_3;
-                out += mainQuadState.pid_rotation_r_out;
+                out -= mainQuadState.pid_rotation_r_out;
+                out -= mainQuadState.pid_rotation_p_out;
+                out += mainQuadState.pid_rotation_y_out;
                 mainQuadState.actual_throttle_3 = out;
             }
             
@@ -239,6 +245,9 @@
                 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;
             }
         }