Diff: TXRX_serial.h

Revision:

5:33abcc31b0aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TXRX_serial.h	Mon May 12 22:58:06 2014 +0000
@@ -0,0 +1,150 @@
+#define TXRX_BUFFER 600
+
+Mutex         TXRX_mutex;
+QuadState     TXRX_txQuadState;
+QuadState     TXRX_rxQuadState;
+Timer         TXRX_tx_dt_timer;
+Timer         TXRX_rx_dt_timer;
+char          TXRX_buffer [TXRX_BUFFER];
+ 
+
+void TXRX_thread_routine (void const *args){ //New magic version
+    DigitalOut led_rx(LED_RX); led_rx = 0;
+    
+    //Prepare state
+    TXRX_txQuadState.reset();
+    TXRX_rxQuadState.reset();
+    
+    //Setup serial
+    Thread::wait(2000);
+    FAST_FLASH_OFF(led_rx,2);
+    RawSerial  TXRX_serial(USBTX, USBRX);
+    FAST_FLASH_OFF(led_rx,2);
+    TXRX_serial.baud(115200);
+    FAST_FLASH_OFF(led_rx,2);
+    Thread::wait(2000);
+    
+    FAST_FLASH_ON(led_rx,5);
+    Thread::wait(5);
+    
+
+    //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;
+    while(1){
+        
+        led_rx = (step % 40 < 35 ? 1 : (actual_rx_dt < 1 ? 1 : 0)); //allways light on while receiving, blink 0.5Hz while not receiving.
+    
+        accept = false;
+        if(TXRX_serial.readable()){
+            //RECEIVE  ... [ ...read this with square brackets included... ] ...
+            bool read = false;
+            memset(TXRX_buffer,0,TXRX_BUFFER); //erase the buffer
+            char * receiving = TXRX_buffer; //receiving pointer
+            int byte; //received byte
+            unsigned int unreadable_count = 0;
+            while(receiving < TXRX_buffer+TXRX_BUFFER && unreadable_count < 1000){
+                if(TXRX_serial.readable()){
+                    unreadable_count = 0;
+                    byte = TXRX_serial.getc();
+                    if(byte < 0)
+                        break;
+                    if(byte == '[')
+                        read = true;
+                    if(read){
+                        *receiving = byte;
+                        ++receiving;
+                        if(byte == ']'){
+                            accept = true;
+                            break;
+                        }
+                    }
+                }else{
+                    ++unreadable_count;
+                    Thread::yield();
+                }
+            }
+            receiving = 0; //terminate the string
+        }
+TXRX_mutex.lock();
+        //PRODUCE ACCEPTING DATA
+        if(accept  &&  /*QuadState::length() == */TXRX_rxQuadState.setFromJSON(TXRX_buffer))
+            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;
+        
+        sent = false;
+        if(TXRX_serial.writeable()){
+            //PREPARE TO SEND
+            memset(TXRX_buffer,0,TXRX_BUFFER);
+            int setted = TXRX_txQuadState.getJSON(TXRX_buffer);
+TXRX_mutex.unlock();
+            if(true || setted == QuadState::length()){
+                //SEND
+                unsigned int unwriteable_count=0;
+                unsigned int to_be_sent = strlen(TXRX_buffer);
+                char * sending = TXRX_buffer;
+                while(to_be_sent > 0  &&  unwriteable_count < 1000){
+                    if(TXRX_serial.writeable()){
+                        unwriteable_count = 0;
+                        if(TXRX_serial.putc(*sending) < 0)
+                            break;
+                        ++sending;
+                        --to_be_sent;
+                    }else{
+                        ++unwriteable_count;
+                        Thread::yield();
+                    }
+                }
+                sent = (to_be_sent == 0 ? true : false);
+            }
+        }
+        
+        led_rx = 0;
+        
+        ///REALX TIMINGS
+        //Sleep the shortest time needed to reach a dt_target; also sleep 30ms 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.030;
+        if(diff_to_sleep > 0)
+            Thread::wait(diff_to_sleep);
+        Thread::wait(30); //NOTE: why?
+        
+        ++step;
+    }
+    //end of while(1)
+}
+
+
+//////////////////////////////////////////////////////
+
+
+bool TXRX_stateExchange (QuadState & tx, QuadState & rx){
+  if(TXRX_mutex.trylock()){
+      rx = TXRX_rxQuadState;
+      TXRX_txQuadState = tx;
+      TXRX_mutex.unlock();
+      return true;
+  }
+  return false;
+}
+
+
+//////////////////////////////////////////////////////