Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
TXRX_nomagic.h
- Committer:
- MatteoT
- Date:
- 2014-05-24
- Revision:
- 7:cda17cffec3c
- Parent:
- 6:4ddd68260f76
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; }