Diff: main.cpp

Revision:

7:cda17cffec3c

Parent:

6:4ddd68260f76

--- a/main.cpp	Tue May 13 22:56:44 2014 +0000
+++ b/main.cpp	Sat May 24 17:42:03 2014 +0000
@@ -52,15 +52,15 @@
     //-------------------------------------------------------------------------------
     
     //Setup ESCs
-    ESC esc1(p23); esc1=0;
-    ESC esc2(p24); esc1=0;
-    ESC esc3(p25); esc1=0;
-    ESC esc4(p26); esc1=0;
+    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;
-    DigitalOut led_esc(LED_ESC); led_esc = 0;
-    DigitalOut led_txrx(LED_TXRX); led_txrx = 0;
+    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
@@ -160,7 +160,7 @@
         QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
         
         //Elaborate inputs to determinate outputs
-        led_esc = 1;
+        //led_esc = 1;
         
         {//Roll PID
             float previous = mainQuadState.pid_rotation_r_error;
@@ -252,6 +252,15 @@
             }
         }
         
+        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
         //-------------------------------------------------------------------------------
@@ -267,7 +276,7 @@
         esc2();
         esc3();
         esc4();
-        led_esc = 0;
+        led_esc = mainQuadState.actual_throttle_1;
         
                
 } //end of 1/8 steps
@@ -276,12 +285,26 @@
         
         //REMOTE INPUT/OUTPUT EXCHANGE
         //-------------------------------------------------------------------------------
-        led_txrx = 1;
+        
         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!!!