Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
TXRX_nomagic.h@7:cda17cffec3c, 2014-05-24 (annotated)
- Committer:
- MatteoT
- Date:
- Sat May 24 17:42:03 2014 +0000
- Revision:
- 7:cda17cffec3c
- Parent:
- 6:4ddd68260f76
experiments (going to first launch)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MatteoT | 5:33abcc31b0aa | 1 | #include "EthernetInterface.h" |
MatteoT | 5:33abcc31b0aa | 2 | |
MatteoT | 5:33abcc31b0aa | 3 | #define TXRX_BUFFER 1024 |
MatteoT | 5:33abcc31b0aa | 4 | #define TXRX_PORT 2225 |
MatteoT | 5:33abcc31b0aa | 5 | #define TXRX_BROADCAST_ADDR "192.168.2.255" |
MatteoT | 5:33abcc31b0aa | 6 | #define TXRX_BROADCAST_PORT 2225 |
MatteoT | 5:33abcc31b0aa | 7 | |
MatteoT | 5:33abcc31b0aa | 8 | |
MatteoT | 5:33abcc31b0aa | 9 | struct QuadStateTXRXTimes{ |
MatteoT | 5:33abcc31b0aa | 10 | double target_tx_dt; |
MatteoT | 5:33abcc31b0aa | 11 | double actual_tx_dt; |
MatteoT | 5:33abcc31b0aa | 12 | double average_tx_dt; |
MatteoT | 5:33abcc31b0aa | 13 | double average_tx_dt_k; |
MatteoT | 5:33abcc31b0aa | 14 | |
MatteoT | 5:33abcc31b0aa | 15 | double target_rx_dt; |
MatteoT | 5:33abcc31b0aa | 16 | double actual_rx_dt; |
MatteoT | 5:33abcc31b0aa | 17 | double average_rx_dt; |
MatteoT | 5:33abcc31b0aa | 18 | double average_rx_dt_k; |
MatteoT | 5:33abcc31b0aa | 19 | |
MatteoT | 5:33abcc31b0aa | 20 | void reset(){ |
MatteoT | 5:33abcc31b0aa | 21 | target_tx_dt = TARGET_TX_DT; |
MatteoT | 5:33abcc31b0aa | 22 | actual_tx_dt = TARGET_TX_DT; |
MatteoT | 5:33abcc31b0aa | 23 | average_tx_dt = TARGET_TX_DT; |
MatteoT | 5:33abcc31b0aa | 24 | average_tx_dt_k = AVERAGE_DT_K_GAIN; |
MatteoT | 5:33abcc31b0aa | 25 | |
MatteoT | 5:33abcc31b0aa | 26 | target_rx_dt = TARGET_RX_DT; |
MatteoT | 5:33abcc31b0aa | 27 | actual_rx_dt = TARGET_RX_DT; |
MatteoT | 5:33abcc31b0aa | 28 | average_rx_dt = TARGET_RX_DT; |
MatteoT | 5:33abcc31b0aa | 29 | average_rx_dt_k = AVERAGE_DT_K_GAIN; |
MatteoT | 5:33abcc31b0aa | 30 | } |
MatteoT | 5:33abcc31b0aa | 31 | }; |
MatteoT | 5:33abcc31b0aa | 32 | |
MatteoT | 5:33abcc31b0aa | 33 | QuadStateTXRXTimes TXRX_times; |
MatteoT | 5:33abcc31b0aa | 34 | Mutex TXRX_mutex; |
MatteoT | 5:33abcc31b0aa | 35 | QuadState TXRX_txQuadState; |
MatteoT | 5:33abcc31b0aa | 36 | QuadState TXRX_rxQuadState; |
MatteoT | 5:33abcc31b0aa | 37 | Timer TXRX_tx_dt_timer; |
MatteoT | 5:33abcc31b0aa | 38 | Timer TXRX_rx_dt_timer; |
MatteoT | 5:33abcc31b0aa | 39 | char TXRX_buffer [TXRX_BUFFER]; |
MatteoT | 5:33abcc31b0aa | 40 | |
MatteoT | 5:33abcc31b0aa | 41 | |
MatteoT | 5:33abcc31b0aa | 42 | void TXRX_thread_routine (void const *args){ |
MatteoT | 5:33abcc31b0aa | 43 | DigitalOut led_rx(LED_RX); led_rx = 0; |
MatteoT | 5:33abcc31b0aa | 44 | |
MatteoT | 5:33abcc31b0aa | 45 | //Prepare state |
MatteoT | 5:33abcc31b0aa | 46 | TXRX_times.reset(); |
MatteoT | 5:33abcc31b0aa | 47 | TXRX_txQuadState.reset(); |
MatteoT | 5:33abcc31b0aa | 48 | TXRX_rxQuadState.reset(); |
MatteoT | 5:33abcc31b0aa | 49 | |
MatteoT | 5:33abcc31b0aa | 50 | //Setup ethernet interface |
MatteoT | 5:33abcc31b0aa | 51 | Thread::wait(2000); |
MatteoT | 5:33abcc31b0aa | 52 | FAST_FLASH_OFF(led_rx,2); |
MatteoT | 5:33abcc31b0aa | 53 | EthernetInterface interface; |
MatteoT | 5:33abcc31b0aa | 54 | FAST_FLASH_OFF(led_rx,2); |
MatteoT | 5:33abcc31b0aa | 55 | interface.init("192.168.2.16","255.255.255.0","192.168.2.1"); |
MatteoT | 5:33abcc31b0aa | 56 | FAST_FLASH_OFF(led_rx,2); |
MatteoT | 6:4ddd68260f76 | 57 | Thread::wait(3000); |
MatteoT | 5:33abcc31b0aa | 58 | |
MatteoT | 5:33abcc31b0aa | 59 | bool connected = false; |
MatteoT | 5:33abcc31b0aa | 60 | while(1){ |
MatteoT | 5:33abcc31b0aa | 61 | |
MatteoT | 5:33abcc31b0aa | 62 | //Setup ethernet connection |
MatteoT | 5:33abcc31b0aa | 63 | while(!connected){ |
MatteoT | 5:33abcc31b0aa | 64 | Thread::wait(100); |
MatteoT | 6:4ddd68260f76 | 65 | FAST_FLASH_OFF(led_rx,3); |
MatteoT | 5:33abcc31b0aa | 66 | connected = interface.connect(3000); |
MatteoT | 5:33abcc31b0aa | 67 | SLOW_FLASH_ON(led_rx,1); |
MatteoT | 5:33abcc31b0aa | 68 | } |
MatteoT | 5:33abcc31b0aa | 69 | FAST_FLASH_OFF(led_rx,2); |
MatteoT | 6:4ddd68260f76 | 70 | Thread::wait(10); |
MatteoT | 5:33abcc31b0aa | 71 | |
MatteoT | 5:33abcc31b0aa | 72 | //Setup RX socket |
MatteoT | 5:33abcc31b0aa | 73 | UDPSocket sock; |
MatteoT | 5:33abcc31b0aa | 74 | sock.bind(TXRX_PORT); |
MatteoT | 6:4ddd68260f76 | 75 | sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.1); |
MatteoT | 5:33abcc31b0aa | 76 | Endpoint server_endpoint; |
MatteoT | 5:33abcc31b0aa | 77 | |
MatteoT | 5:33abcc31b0aa | 78 | //Setup broadcast socket |
MatteoT | 5:33abcc31b0aa | 79 | /*UDPSocket response_sock; |
MatteoT | 5:33abcc31b0aa | 80 | response_sock.init(); |
MatteoT | 5:33abcc31b0aa | 81 | response_sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.25); |
MatteoT | 5:33abcc31b0aa | 82 | response_*/sock.set_broadcasting(); |
MatteoT | 5:33abcc31b0aa | 83 | Endpoint broadcast; |
MatteoT | 5:33abcc31b0aa | 84 | broadcast.set_address(TXRX_BROADCAST_ADDR, TXRX_BROADCAST_PORT); |
MatteoT | 5:33abcc31b0aa | 85 | |
MatteoT | 5:33abcc31b0aa | 86 | //Ready! |
MatteoT | 5:33abcc31b0aa | 87 | FAST_FLASH_ON(led_rx,5); |
MatteoT | 6:4ddd68260f76 | 88 | Thread::wait(10); |
MatteoT | 5:33abcc31b0aa | 89 | |
MatteoT | 5:33abcc31b0aa | 90 | //Prepare timers |
MatteoT | 5:33abcc31b0aa | 91 | TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start(); |
MatteoT | 5:33abcc31b0aa | 92 | TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start(); |
MatteoT | 5:33abcc31b0aa | 93 | |
MatteoT | 5:33abcc31b0aa | 94 | |
MatteoT | 5:33abcc31b0aa | 95 | //Main tx/rx loop |
MatteoT | 5:33abcc31b0aa | 96 | //==================================================================== |
MatteoT | 5:33abcc31b0aa | 97 | |
MatteoT | 5:33abcc31b0aa | 98 | int rx_result=-1, tx_result=-1; |
MatteoT | 5:33abcc31b0aa | 99 | unsigned short int step=0; |
MatteoT | 5:33abcc31b0aa | 100 | while(connected){ |
MatteoT | 5:33abcc31b0aa | 101 | |
MatteoT | 5:33abcc31b0aa | 102 | led_rx = (step % 40 < 35 ? 1 : (TXRX_times.actual_rx_dt < .5 ? 1 : 0)); //allways light on while receiving, slow blink while not receiving. |
MatteoT | 5:33abcc31b0aa | 103 | |
MatteoT | 5:33abcc31b0aa | 104 | //Receive message on buffer |
MatteoT | 5:33abcc31b0aa | 105 | memset(TXRX_buffer,0,TXRX_BUFFER); //zero the buffer |
MatteoT | 5:33abcc31b0aa | 106 | rx_result = sock.receiveFrom(server_endpoint, TXRX_buffer, TXRX_BUFFER); |
MatteoT | 5:33abcc31b0aa | 107 | |
MatteoT | 5:33abcc31b0aa | 108 | TXRX_mutex.lock(); |
MatteoT | 5:33abcc31b0aa | 109 | |
MatteoT | 5:33abcc31b0aa | 110 | //Consume rx message |
MatteoT | 5:33abcc31b0aa | 111 | if(rx_result > 0) |
MatteoT | 5:33abcc31b0aa | 112 | rx_result = TXRX_rxQuadState.setFromJSON(TXRX_buffer); |
MatteoT | 5:33abcc31b0aa | 113 | |
MatteoT | 5:33abcc31b0aa | 114 | //Import tx/rx times config from main thread |
MatteoT | 5:33abcc31b0aa | 115 | TXRX_times.target_rx_dt = TXRX_txQuadState.target_rx_dt; |
MatteoT | 5:33abcc31b0aa | 116 | TXRX_times.average_rx_dt_k = TXRX_txQuadState.average_rx_dt_k; |
MatteoT | 5:33abcc31b0aa | 117 | TXRX_times.target_tx_dt = TXRX_txQuadState.target_tx_dt; |
MatteoT | 5:33abcc31b0aa | 118 | TXRX_times.average_tx_dt_k = TXRX_txQuadState.average_tx_dt_k; |
MatteoT | 5:33abcc31b0aa | 119 | //Export tx/rx times measured to remote and main thread |
MatteoT | 5:33abcc31b0aa | 120 | TXRX_txQuadState.actual_rx_dt = (TXRX_rxQuadState.actual_rx_dt = TXRX_times.actual_rx_dt); |
MatteoT | 5:33abcc31b0aa | 121 | TXRX_txQuadState.average_rx_dt = (TXRX_rxQuadState.average_rx_dt = TXRX_times.average_rx_dt); |
MatteoT | 5:33abcc31b0aa | 122 | TXRX_txQuadState.actual_tx_dt = (TXRX_rxQuadState.actual_tx_dt = TXRX_times.actual_tx_dt); |
MatteoT | 5:33abcc31b0aa | 123 | TXRX_txQuadState.average_tx_dt = (TXRX_rxQuadState.average_tx_dt = TXRX_times.average_tx_dt); |
MatteoT | 5:33abcc31b0aa | 124 | |
MatteoT | 5:33abcc31b0aa | 125 | //Write broadcast message on buffer |
MatteoT | 5:33abcc31b0aa | 126 | TXRX_txQuadState.getJSON(TXRX_buffer); |
MatteoT | 5:33abcc31b0aa | 127 | |
MatteoT | 5:33abcc31b0aa | 128 | TXRX_mutex.unlock(); |
MatteoT | 5:33abcc31b0aa | 129 | |
MatteoT | 5:33abcc31b0aa | 130 | //Send broadcast response |
MatteoT | 5:33abcc31b0aa | 131 | tx_result = /*response_*/sock.sendTo(broadcast,TXRX_buffer,strlen(TXRX_buffer)); |
MatteoT | 5:33abcc31b0aa | 132 | |
MatteoT | 5:33abcc31b0aa | 133 | //check rx/tx dt time |
MatteoT | 5:33abcc31b0aa | 134 | if(rx_result > 0) |
MatteoT | 5:33abcc31b0aa | 135 | QUAD_STATE_UPDATE_DT (TXRX_times, rx, TXRX_rx_dt_timer) |
MatteoT | 5:33abcc31b0aa | 136 | else |
MatteoT | 5:33abcc31b0aa | 137 | QUAD_STATE_READ_ACTUAL_DT (TXRX_times, rx, TXRX_rx_dt_timer) |
MatteoT | 5:33abcc31b0aa | 138 | if(tx_result > 0) |
MatteoT | 5:33abcc31b0aa | 139 | QUAD_STATE_UPDATE_DT (TXRX_times, tx, TXRX_tx_dt_timer) |
MatteoT | 5:33abcc31b0aa | 140 | else |
MatteoT | 5:33abcc31b0aa | 141 | QUAD_STATE_READ_ACTUAL_DT (TXRX_times, tx, TXRX_tx_dt_timer) |
MatteoT | 5:33abcc31b0aa | 142 | |
MatteoT | 5:33abcc31b0aa | 143 | //check link when receiving nothing |
MatteoT | 6:4ddd68260f76 | 144 | if(TXRX_times.actual_rx_dt > 10.0*TXRX_times.target_rx_dt){ |
MatteoT | 5:33abcc31b0aa | 145 | interface.disconnect(); |
MatteoT | 5:33abcc31b0aa | 146 | connected = false; |
MatteoT | 5:33abcc31b0aa | 147 | } |
MatteoT | 5:33abcc31b0aa | 148 | ++step; |
MatteoT | 5:33abcc31b0aa | 149 | |
MatteoT | 5:33abcc31b0aa | 150 | led_rx = 0; |
MatteoT | 5:33abcc31b0aa | 151 | |
MatteoT | 5:33abcc31b0aa | 152 | //Sleep |
MatteoT | 5:33abcc31b0aa | 153 | double to_sleep = (TXRX_times.target_tx_dt - TXRX_times.actual_tx_dt); |
MatteoT | 5:33abcc31b0aa | 154 | if(to_sleep < 0.010) |
MatteoT | 5:33abcc31b0aa | 155 | to_sleep=0.010; |
MatteoT | 5:33abcc31b0aa | 156 | QUAD_STATE_WAIT_DT_TARGET(0, to_sleep) |
MatteoT | 5:33abcc31b0aa | 157 | |
MatteoT | 5:33abcc31b0aa | 158 | }//end of while(connected) |
MatteoT | 5:33abcc31b0aa | 159 | |
MatteoT | 5:33abcc31b0aa | 160 | }//end of while(1) |
MatteoT | 5:33abcc31b0aa | 161 | } |
MatteoT | 5:33abcc31b0aa | 162 | |
MatteoT | 5:33abcc31b0aa | 163 | bool TXRX_stateExchange (QuadState & tx, QuadState & rx){ |
MatteoT | 5:33abcc31b0aa | 164 | if(TXRX_mutex.trylock()){ |
MatteoT | 5:33abcc31b0aa | 165 | rx = TXRX_rxQuadState; |
MatteoT | 5:33abcc31b0aa | 166 | TXRX_txQuadState = tx; |
MatteoT | 5:33abcc31b0aa | 167 | TXRX_mutex.unlock(); |
MatteoT | 5:33abcc31b0aa | 168 | return true; |
MatteoT | 5:33abcc31b0aa | 169 | } |
MatteoT | 5:33abcc31b0aa | 170 | return false; |
MatteoT | 5:33abcc31b0aa | 171 | } |
MatteoT | 5:33abcc31b0aa | 172 |