File content as of revision 7:cda17cffec3c:

#include "EthernetInterface.h"

#define TXRX_BUFFER 1024
#define TXRX_PORT   2225
#define TXRX_BROADCAST_ADDR   "192.168.2.255"
#define TXRX_BROADCAST_PORT   2225
 

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;
Timer                TXRX_tx_dt_timer;
Timer                TXRX_rx_dt_timer;
char                 TXRX_buffer [TXRX_BUFFER];
 
 
void TXRX_thread_routine (void const *args){
  DigitalOut led_rx(LED_RX); led_rx = 0;
 
  //Prepare state
  TXRX_times.reset();
  TXRX_txQuadState.reset();
  TXRX_rxQuadState.reset();
  
  //Setup ethernet interface
  Thread::wait(2000);
  FAST_FLASH_OFF(led_rx,2);
  EthernetInterface interface;
  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(3000);
  
  bool connected = false;
  while(1){

      //Setup ethernet connection
      while(!connected){
        Thread::wait(100);
        FAST_FLASH_OFF(led_rx,3);
        connected = interface.connect(3000);
        SLOW_FLASH_ON(led_rx,1);
      }
      FAST_FLASH_OFF(led_rx,2);
      Thread::wait(10);
  
      //Setup RX socket
      UDPSocket sock;
      sock.bind(TXRX_PORT);
      sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.1);
      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.25);
      response_*/sock.set_broadcasting();
      Endpoint broadcast;
      broadcast.set_address(TXRX_BROADCAST_ADDR, TXRX_BROADCAST_PORT);
     
      //Ready!
      FAST_FLASH_ON(led_rx,5);
      Thread::wait(10);
      
      //Prepare timers
      TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
      TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
      
      
      //Main tx/rx loop
      //====================================================================
      
      int rx_result=-1, tx_result=-1;
      unsigned short int step=0;
      while(connected){
        
        led_rx = (step % 40 < 35 ? 1 : (TXRX_times.actual_rx_dt < .5 ? 1 : 0)); //allways light on while receiving, slow blink while not receiving.
        
        //Receive message on buffer
        memset(TXRX_buffer,0,TXRX_BUFFER); //zero the buffer
        rx_result = sock.receiveFrom(server_endpoint, TXRX_buffer, TXRX_BUFFER);
        
TXRX_mutex.lock();
     
        //Consume rx message
        if(rx_result > 0)
            rx_result = TXRX_rxQuadState.setFromJSON(TXRX_buffer);
            
        //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);
        
        //Write broadcast message on buffer
        TXRX_txQuadState.getJSON(TXRX_buffer);
        
TXRX_mutex.unlock();
     
        //Send broadcast response
        tx_result = /*response_*/sock.sendTo(broadcast,TXRX_buffer,strlen(TXRX_buffer));
        
        //check rx/tx dt time
        if(rx_result > 0)
          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(tx_result > 0)
          QUAD_STATE_UPDATE_DT (TXRX_times, tx, TXRX_tx_dt_timer)
        else
          QUAD_STATE_READ_ACTUAL_DT (TXRX_times, tx, TXRX_tx_dt_timer)
        
        //check link when receiving nothing
        if(TXRX_times.actual_rx_dt > 10.0*TXRX_times.target_rx_dt){
            interface.disconnect();
            connected = false;
        }
        ++step;
        
        led_rx = 0;
        
        //Sleep
        double to_sleep = (TXRX_times.target_tx_dt - TXRX_times.actual_tx_dt);
        if(to_sleep < 0.010)
            to_sleep=0.010;
        QUAD_STATE_WAIT_DT_TARGET(0, to_sleep)
        
      }//end of while(connected)
      
    }//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;
}