Revision 6:4ddd68260f76, committed 2014-05-13

Comitter:

MatteoT

Date:

Tue May 13 22:56:44 2014 +0000

Parent:

5:33abcc31b0aa

Child:

7:cda17cffec3c

Commit message:

experiments (magic networking send/receive working)

Changed in this revision

--- a/TXRX_magic.h	Mon May 12 22:58:06 2014 +0000
+++ b/TXRX_magic.h	Tue May 13 22:56:44 2014 +0000
@@ -7,11 +7,13 @@
 #define MSG_LEN (TXRX_BUFFER-14-20-8-10)
 #define IP_LEN_OFFSET (2)
 #define IP_PROTO_OFFSET (9)
+#define IP_CKS_OFFSET (10)
 #define IP_SRC_ADDR_OFFSET (12)
 #define IP_DEST_ADDR_OFFSET (16)
 #define IP_PAYLOAD_OFFSET (20)
 #define IP_UDP_DESTPORT_OFFSET (20+2)
 #define IP_UDP_LEN_OFFSET (20+2+2)
+#define IP_UDP_CKS_OFFSET (20+2+2+2)
 #define IP_UDP_PAYLOAD_OFFSET (20+8)
 //ARP:
 #define ARP_SENDER_HW_ADDR_OFFSET (8)
@@ -19,6 +21,30 @@
 #define ARP_LEN (28)
 
 /////////////////////////////////////////////////////////////////
+struct QuadStateTXRXTimes{
+    double    target_tx_dt;
+    double    actual_tx_dt;
+    double    average_tx_dt;
+    double    average_tx_dt_k;
+
+    double    target_rx_dt;
+    double    actual_rx_dt;
+    double    average_rx_dt;
+    double    average_rx_dt_k;
+    
+    void reset(){
+        target_tx_dt = TARGET_TX_DT;
+        actual_tx_dt = TARGET_TX_DT;
+        average_tx_dt = TARGET_TX_DT;
+        average_tx_dt_k = AVERAGE_DT_K_GAIN;
+
+        target_rx_dt = TARGET_RX_DT;
+        actual_rx_dt = TARGET_RX_DT;
+        average_rx_dt = TARGET_RX_DT;
+        average_rx_dt_k = AVERAGE_DT_K_GAIN;
+    }
+};
+QuadStateTXRXTimes TXRX_times;
 Mutex         TXRX_mutex;
 QuadState     TXRX_txQuadState;
 QuadState     TXRX_rxQuadState;
@@ -73,10 +99,15 @@
     DEST_IP_ADDR                     //Target proto addr
 };
 
+#define htons(n)  ( (((n) & 0xFF00) >> 8) | (((n) & 0x00FF) << 8) )
+uint16_t ip_checksum (const char * buf, size_t hdr_len);
+uint16_t udp_checksum(const char * buff, size_t len, const char * src_addr, const char * dest_addr);
+
 void TXRX_thread_routine (void const *args){ //New magic version
     DigitalOut led_rx(LED_RX); led_rx = 0;
     
     //Prepare state
+    TXRX_times.reset();
     TXRX_txQuadState.reset();
     TXRX_rxQuadState.reset();
     
@@ -97,10 +128,10 @@
         while(!eth0.link()){
             SLOW_FLASH_ON(led_rx,1);
         };
-        Thread::wait(1);
+        Thread::wait(5);
         FAST_FLASH_OFF(led_rx,2);
         
-        //Perform 5 gratuitous ARP reply
+        //Perform 5 gratuitous ARP reply, with an interval of 10ms
         memset(ethernet_frame, 0xff, 6); //ethernet broadcast
         memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw addr = my mac; sender ip addr predefined.
         memcpy(arp_frame+ARP_TARGET_HW_ADDR_OFFSET, arp_frame+ARP_SENDER_HW_ADDR_OFFSET, 6+4); //target = sender (both hw and ip - gratuitous ARP reply as announcement).
@@ -118,26 +149,23 @@
         //Prepare timers
         TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
         TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
-        
-        //Data to be setted into thread-safe area.
-        float actual_tx_dt, target_tx_dt, actual_rx_dt, target_rx_dt;
     
         unsigned int step=0;
         bool accept=false, sent=false, connected=true;
         while(connected){
             
-            led_rx = (step % 40 < 35 ? 1 : (actual_rx_dt < 1 ? 1 : 0)); //allways light on while receiving, blink 0.5Hz while not receiving.
+            led_rx = (step % 40 < 35 ? 1 : (TXRX_times.actual_rx_dt < 1 ? 1 : 0)); //allways light on while receiving, blink 0.5Hz while not receiving.
             
             {   //RECEIVE
                 accept = false;
                 const int received = eth0.receive();
                 if(received > ETH_PAYLOAD_OFFSET && received < TXRX_BUFFER){ //check ethernet frame packet
                     //ACCEPTANCE
-                    accept = (eth0.read(TXRX_buffer,received) ==  received ? true : false);
+                    accept = (eth0.read(TXRX_buffer,received) > ETH_PAYLOAD_OFFSET ? true : false);
                     //memcpy(ethernet_frame, TXRX_buffer+6, 6);//sent dest hw addr = received src hw addr
                     //CHECK ARP PACKET AND SEND REPLY
                     if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame_arp_proto,2)){
-                        accept = false; //consider arp packets as not accepted.
+                        accept = false; //consider arp packets as not accepted and reply now.
                         memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw = my mac
                         memcpy(arp_frame+ARP_TARGET_HW_ADDR_OFFSET, TXRX_buffer+ETH_PAYLOAD_OFFSET+ARP_SENDER_HW_ADDR_OFFSET, 6+4); //target = sender (both hw and ip)
                         eth0.write(ethernet_frame,12);
@@ -145,60 +173,79 @@
                         eth0.write(arp_frame,ARP_LEN);
                         eth0.send();
                     }
-                    else if(accept && received > ETH_PAYLOAD_OFFSET + IP_UDP_PAYLOAD_OFFSET){
+                    else if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET, ethernet_frame+ETH_PROTO_OFFSET,2)
+                      && received > ETH_PAYLOAD_OFFSET + IP_UDP_PAYLOAD_OFFSET + 2){
                         //VALIDATE UDP PACKET
                         TXRX_buffer[received] = 0; //string termination
+                        accept = true;/*
                         if(0 != memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame+ETH_PROTO_OFFSET, 2)) accept = false;
                         if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_PROTO_OFFSET,ipudp_frame+IP_PROTO_OFFSET, 2)) accept = false;
-                        if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_DESTPORT_OFFSET,ipudp_frame+IP_UDP_DESTPORT_OFFSET, 2)) accept = false;
+                        if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_DESTPORT_OFFSET,ipudp_frame+IP_UDP_DESTPORT_OFFSET, 2)) accept = false;*/
                     }
                     else accept = false;
                 }
             }
 TXRX_mutex.lock();
             //PRODUCE ACCEPTING DATA
-            if(accept  &&  QuadState::length() == TXRX_rxQuadState.setFromJSON(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_PAYLOAD_OFFSET))
-                QUAD_STATE_UPDATE_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
-            else
-                QUAD_STATE_READ_ACTUAL_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
-            actual_rx_dt = TXRX_rxQuadState.actual_rx_dt;
-            target_rx_dt = TXRX_rxQuadState.target_rx_dt;
-            
-            //TIME MEASURE SENT DATA
-            if(sent)
-                QUAD_STATE_UPDATE_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
-            else
-                QUAD_STATE_READ_ACTUAL_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
-            actual_tx_dt = TXRX_rxQuadState.actual_tx_dt;
-            target_tx_dt = TXRX_rxQuadState.target_tx_dt;
+            if(accept)
+                accept = (QuadState::length() == TXRX_rxQuadState.setFromJSON(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_PAYLOAD_OFFSET));
+                
+            //Import tx/rx times config from main thread
+            TXRX_times.target_rx_dt        =  TXRX_txQuadState.target_rx_dt;
+            TXRX_times.average_rx_dt_k     =  TXRX_txQuadState.average_rx_dt_k;
+            TXRX_times.target_tx_dt        =  TXRX_txQuadState.target_tx_dt;
+            TXRX_times.average_tx_dt_k     =  TXRX_txQuadState.average_tx_dt_k;
+            //Export tx/rx times measured to remote and main thread
+            TXRX_txQuadState.actual_rx_dt  = (TXRX_rxQuadState.actual_rx_dt  = TXRX_times.actual_rx_dt);
+            TXRX_txQuadState.average_rx_dt = (TXRX_rxQuadState.average_rx_dt = TXRX_times.average_rx_dt);
+            TXRX_txQuadState.actual_tx_dt  = (TXRX_rxQuadState.actual_tx_dt  = TXRX_times.actual_tx_dt);
+            TXRX_txQuadState.average_tx_dt = (TXRX_rxQuadState.average_tx_dt = TXRX_times.average_tx_dt);
             
             {   //SEND
-                char json [MSG_LEN];
-                memset(json,' ',MSG_LEN);
-                int setted = TXRX_txQuadState.getJSON(json);
+                //TXRX_buffer will contain: IP header, UDP header, json message. (no ethernet header).
+                //Why? the function udp_checksum needs the udp payload to be next to the udp header.
+                memset(TXRX_buffer+IP_UDP_PAYLOAD_OFFSET,' ',MSG_LEN);
+                int setted = TXRX_txQuadState.getJSON(TXRX_buffer+IP_UDP_PAYLOAD_OFFSET);
 TXRX_mutex.unlock();
-                if(true || setted == QuadState::length()){
-                    //WRITE PACKET AND SEND
-                    eth0.write(ethernet_frame,ETH_PAYLOAD_OFFSET);
-                    eth0.write(ipudp_frame,IP_UDP_PAYLOAD_OFFSET);
-                    eth0.write(json,MSG_LEN);
-                    sent = (eth0.send() ? true : false);
-                }
+                //CALC CHECKSUMs
+                memcpy(TXRX_buffer,ipudp_frame,IP_UDP_PAYLOAD_OFFSET);
+                memset(TXRX_buffer+IP_CKS_OFFSET,0,2);
+                memset(TXRX_buffer+IP_UDP_CKS_OFFSET,0,2);
+                uint16_t ip_cks  = htons ( ip_checksum(TXRX_buffer,IP_PAYLOAD_OFFSET) );
+                //uint16_t udp_cks = htons ( udp_checksum(TXRX_buffer+IP_PAYLOAD_OFFSET, 8+MSG_LEN, TXRX_buffer+IP_SRC_ADDR_OFFSET, TXRX_buffer+IP_DEST_ADDR_OFFSET) );
+                memcpy(TXRX_buffer+IP_CKS_OFFSET,&ip_cks,2);
+                //memcpy(TXRX_buffer+IP_UDP_CKS_OFFSET,&udp_cks,2);
+                //WRITE PACKET AND SEND
+                eth0.write(ethernet_frame,ETH_PAYLOAD_OFFSET);
+                eth0.write(TXRX_buffer,IP_UDP_PAYLOAD_OFFSET+MSG_LEN);
+                sent = (eth0.send() ? true : false);
             }
             
+            //check rx/tx dt time
+            if(accept)
+              QUAD_STATE_UPDATE_DT (TXRX_times, rx, TXRX_rx_dt_timer)
+            else
+              QUAD_STATE_READ_ACTUAL_DT (TXRX_times, rx, TXRX_rx_dt_timer)
+            if(sent)
+              QUAD_STATE_UPDATE_DT (TXRX_times, tx, TXRX_tx_dt_timer)
+            else
+              QUAD_STATE_READ_ACTUAL_DT (TXRX_times, tx, TXRX_tx_dt_timer)
+            
+            
+            if(TXRX_times.actual_rx_dt > 10.0*TXRX_times.target_rx_dt)//disconnect when receiving nothing for a long time
+                connected = false;
+            if(TXRX_times.actual_rx_dt > 3.00*TXRX_times.target_rx_dt)//check link when receiving nothing for just a while
+                connected = eth0.link();
+                
             led_rx = 0;
             
-            ///REALX TIMINGS
-            //Sleep the shortest time needed to reach a dt_target; also sleep 20ms anyway.
-            double diff_tx = target_tx_dt - actual_tx_dt;
-            //double diff_rx = target_rx_dt - actual_rx_dt;
-            double diff_to_sleep = /*min(diff_rx,*/ diff_tx/*)*/ - 0.020;
-            if(diff_to_sleep > 0)
-                Thread::wait(diff_to_sleep);
-            Thread::wait(20); //NOTE: why?
+            ///REALX TIMINGS (always sleep at least 48ms)
+            double to_sleep = (TXRX_times.target_tx_dt - TXRX_times.actual_tx_dt) - 0.048;
+            if(to_sleep < 0)
+                to_sleep=0;
+            QUAD_STATE_WAIT_DT_TARGET(0, to_sleep)
+            Thread::wait(48);
             
-            if(actual_rx_dt > 1)//check link when receiving nothing
-                connected = eth0.link();
             ++step;
         }
         //end of while(connected)
@@ -225,160 +272,42 @@
 
 
 
-
-
-
-/*
-
- //#define TXRX_PORT   2224
- //#define TXRX_BROADCAST_ADDR   "192.168.2.255"
- //#define TXRX_BROADCAST_PORT   2225
- 
-char          TXRX_buffer [TXRX_BUFFER];
-void TXRX_thread_routine (void const *args){
-  DigitalOut led_rx(LED_RX); led_rx = 0;
-  
-  
-  //Setup RX socket
-  UDPSocket sock;
-  sock.bind(TXRX_PORT);
-  sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.5);
-  //sock.set_broadcasting();
-  Endpoint server_endpoint;
-
-  Setup broadcast socket
-  UDPSocket response_sock;
-  response_sock.init();
-  response_sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.5);
-  response_sock.set_broadcasting();
-  Endpoint broadcast;
-  broadcast.set_address(TXRX_BROADCAST_ADDR, TXRX_BROADCAST_PORT);
+uint16_t ip_checksum (const char * buf, size_t hdr_len)
+{
+        unsigned long sum = 0;
+        const char *ip1 = buf;
+        while (hdr_len > 1)
+        {
+                sum += (*ip1++)<<8;  sum += (*ip1++);
+                if (sum & 0x80000000)
+                        sum = (sum & 0xFFFF) + (sum >> 16);
+                hdr_len -= 2;
+        }
+        while (sum >> 16)
+                sum = (sum & 0xFFFF) + (sum >> 16);
+        return(~sum);
+}
 
-  
-  
-  //Prepare state
-  TXRX_txQuadState.reset();
-  TXRX_rxQuadState.reset();
-  
-  //Prepare timers
-  TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
-  TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
-  
-    
-  //Data to be setted into thread-safe area.
-  double actual_tx_dt, target_tx_dt;
-  int rx_result=-1, tx_result=-1;
-  //Data to be setted from thread-safe area.
-  double actual_rx_dt, target_rx_dt;
-  
-  
-  
-  //Main tx/rx loop
-  //====================================================================
-  
-  led_rx = 1; //say it's ready!
-  
-  while(true){
-    
-    
-    //Receive
-    std::fill(TXRX_buffer, TXRX_buffer+TXRX_BUFFER, 0); //zero the buffer
-    rx_result = sock.receiveFrom(server_endpoint, TXRX_buffer, TXRX_BUFFER);
-
-TXRX_mutex.lock();
-
-    led_rx = 1;
-
-    //check rx time
-    if(rx_result > 0   &&
-        TXRX_rxQuadState.setFromJSON(TXRX_buffer) > 0
-    ){
-      QUAD_STATE_UPDATE_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
-    }else{
-      QUAD_STATE_READ_ACTUAL_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
+uint16_t udp_checksum(const char * buff, size_t len, const char * src_addr, const char * dest_addr){
+    const char *buf=buff;
+    uint32_t sum;
+    size_t length=len;
+    sum = 0;
+    while (len > 1){
+        sum += (*buf++)<<8;  sum += (*buf++);
+        if (sum & 0x80000000)
+            sum = (sum & 0xFFFF) + (sum >> 16);
+        len -= 2;
     }
-    actual_rx_dt = TXRX_rxQuadState.actual_rx_dt;
-    target_rx_dt = TXRX_rxQuadState.target_rx_dt;
-    
-    //check previous tx time
-    if(tx_result > 0){
-      QUAD_STATE_UPDATE_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
-    }else{
-      QUAD_STATE_READ_ACTUAL_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
-    }
-    actual_tx_dt = TXRX_rxQuadState.actual_tx_dt;
-    target_tx_dt = TXRX_rxQuadState.target_tx_dt;
-    
-    
-    //Prepare broadcast response  
-    unsigned int tx_len;
-    {
-        std::string tx_str = "@";TXRX_txQuadState.getJSON();
-    
-TXRX_mutex.unlock();
-
-        tx_str = "\n\n"+tx_str+"\n\0";
-        tx_len = tx_str.length();
-        if(tx_len > TXRX_BUFFER) tx_len = TXRX_BUFFER;
-        memcpy(TXRX_buffer, tx_str.c_str(), tx_len );
-    }
-    //Send broadcast response
-    tx_result = response_sock.sendTo(broadcast,TXRX_buffer,tx_len);
-    
-    led_rx = 0;
-    
-    //Sleep the shortest time needed to reach a dt_target; also sleep 5ms anyway.
-    double diff_tx = target_tx_dt - actual_tx_dt;
-    double diff_rx = target_rx_dt - actual_rx_dt;
-    double diff_to_sleep = min(diff_rx, diff_tx) - 0.005;
-    QUAD_STATE_WAIT_DT_TARGET(0, diff_to_sleep);
-    Thread::wait(40); //NOTE: why?
-  }
-}*/
-
-
-
-
-    /*
-      char state = 'N';
-      char name[TXRX_BUFFER]; //N
-      char value[TXRX_BUFFER]; //v
-      unsigned int i=0;
-      unsigned int name_i=0;
-      unsigned int value_i=0;
-      
-      bzero(name, TXRX_BUFFER);
-      bzero(value, TXRX_BUFFER);
-      
-      TXRX_mutex.lock();
-      
-      while(i < TXRX_BUFFER){
-        switch(state){
-            case 'N':
-                if(buffer[i] == ':'){
-                    state = 'v';
-                    value_i = 0;
-                    bzero(value, TXRX_BUFFER);
-                }
-                else
-                    name[name_i++] = buffer[i];
-                break;
-            case 'v':
-                if(buffer[i] == '\r' || buffer[i] == '\n'){
-                    //End of name:value pair
-                    void* var = TXRX_rxQuadState[name];
-                    if(var != NULL){
-                        *var = 
-                    }
-                    
-                    //prepare for next pair
-                    state = 'N';
-                    name_i = 0;
-                    bzero(name, TXRX_BUFFER);
-                }
-                else
-                    value[value_i++] = buffer[i];
-                break;
-        }
-        ++i;
-      }*/
\ No newline at end of file
+    if ( len & 1 )
+        sum += *((uint8_t *)buf);
+    sum += (*src_addr++)<<8;   sum += (*src_addr++);
+    sum += (*src_addr++)<<8;   sum += (*src_addr++);
+    sum += (*dest_addr++)<<8;  sum += (*dest_addr++);
+    sum += (*dest_addr++)<<8;  sum += (*dest_addr++);
+    sum += htons(17); //IPPROTO_UDP
+    sum += htons(length);
+    while (sum >> 16)
+            sum = (sum & 0xFFFF) + (sum >> 16);
+    return ((uint16_t)(~sum));
+}
\ No newline at end of file

--- a/TXRX_nomagic.h	Mon May 12 22:58:06 2014 +0000
+++ b/TXRX_nomagic.h	Tue May 13 22:56:44 2014 +0000
@@ -54,7 +54,7 @@
   FAST_FLASH_OFF(led_rx,2);
   interface.init("192.168.2.16","255.255.255.0","192.168.2.1");
   FAST_FLASH_OFF(led_rx,2);
-  Thread::wait(2000);
+  Thread::wait(3000);
   
   bool connected = false;
   while(1){
@@ -62,19 +62,17 @@
       //Setup ethernet connection
       while(!connected){
         Thread::wait(100);
-        FAST_FLASH_OFF(led_rx,2);
+        FAST_FLASH_OFF(led_rx,3);
         connected = interface.connect(3000);
-        FAST_FLASH_OFF(led_rx,2);
-        Thread::wait(100);
         SLOW_FLASH_ON(led_rx,1);
       }
       FAST_FLASH_OFF(led_rx,2);
-      Thread::wait(100);
+      Thread::wait(10);
   
       //Setup RX socket
       UDPSocket sock;
       sock.bind(TXRX_PORT);
-      sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.25);
+      sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.1);
       Endpoint server_endpoint;
      
       //Setup broadcast socket
@@ -87,7 +85,7 @@
      
       //Ready!
       FAST_FLASH_ON(led_rx,5);
-      Thread::wait(100);
+      Thread::wait(10);
       
       //Prepare timers
       TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
@@ -143,7 +141,7 @@
           QUAD_STATE_READ_ACTUAL_DT (TXRX_times, tx, TXRX_tx_dt_timer)
         
         //check link when receiving nothing
-        if(TXRX_times.actual_rx_dt > 3){
+        if(TXRX_times.actual_rx_dt > 10.0*TXRX_times.target_rx_dt){
             interface.disconnect();
             connected = false;
         }

--- 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;
             }
         }

--- a/state.h	Mon May 12 22:58:06 2014 +0000
+++ b/state.h	Tue May 13 22:56:44 2014 +0000
@@ -335,7 +335,7 @@
 //#define JSON_GET(var)   array_vector.push_back(picojson::value(var));
 #define JSON_GET(var) ,var
 
-        return sprintf(buffer, "[%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lf,%lg]\r\n"
+        return sprintf(buffer, "[%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lf,%lg]   \r\n"
 
         JSON_GET(reference_throttle_1)
         JSON_GET(reference_throttle_2)