File content as of revision 7:cda17cffec3c:

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


//////////////////////////////////////////////////////