Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

TXRX_nomagic.h@6:4ddd68260f76, 2014-05-13 (annotated)

Committer:
MatteoT
Date:
Tue May 13 22:56:44 2014 +0000
Revision:
6:4ddd68260f76
Parent:
5:33abcc31b0aa 
experiments (magic networking send/receive working)

Who changed what in which revision?

UserRevisionLine numberNew 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