Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

TXRX_magic.h@5:33abcc31b0aa, 2014-05-12 (annotated)

Committer:
MatteoT
Date:
Mon May 12 22:58:06 2014 +0000
Revision:
5:33abcc31b0aa
Child:
6:4ddd68260f76
experiments (txrx and imu working)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MatteoT 5:33abcc31b0aa 1 ////////////// Information about the frame format: //////////////
MatteoT 5:33abcc31b0aa 2 #define TXRX_BUFFER 600
MatteoT 5:33abcc31b0aa 3 //Ethernet:
MatteoT 5:33abcc31b0aa 4 #define ETH_PROTO_OFFSET (12)
MatteoT 5:33abcc31b0aa 5 #define ETH_PAYLOAD_OFFSET (14)
MatteoT 5:33abcc31b0aa 6 //IP/UDP:
MatteoT 5:33abcc31b0aa 7 #define MSG_LEN (TXRX_BUFFER-14-20-8-10)
MatteoT 5:33abcc31b0aa 8 #define IP_LEN_OFFSET (2)
MatteoT 5:33abcc31b0aa 9 #define IP_PROTO_OFFSET (9)
MatteoT 5:33abcc31b0aa 10 #define IP_SRC_ADDR_OFFSET (12)
MatteoT 5:33abcc31b0aa 11 #define IP_DEST_ADDR_OFFSET (16)
MatteoT 5:33abcc31b0aa 12 #define IP_PAYLOAD_OFFSET (20)
MatteoT 5:33abcc31b0aa 13 #define IP_UDP_DESTPORT_OFFSET (20+2)
MatteoT 5:33abcc31b0aa 14 #define IP_UDP_LEN_OFFSET (20+2+2)
MatteoT 5:33abcc31b0aa 15 #define IP_UDP_PAYLOAD_OFFSET (20+8)
MatteoT 5:33abcc31b0aa 16 //ARP:
MatteoT 5:33abcc31b0aa 17 #define ARP_SENDER_HW_ADDR_OFFSET (8)
MatteoT 5:33abcc31b0aa 18 #define ARP_TARGET_HW_ADDR_OFFSET (8+6+4)
MatteoT 5:33abcc31b0aa 19 #define ARP_LEN (28)
MatteoT 5:33abcc31b0aa 20
MatteoT 5:33abcc31b0aa 21 /////////////////////////////////////////////////////////////////
MatteoT 5:33abcc31b0aa 22 Mutex TXRX_mutex;
MatteoT 5:33abcc31b0aa 23 QuadState TXRX_txQuadState;
MatteoT 5:33abcc31b0aa 24 QuadState TXRX_rxQuadState;
MatteoT 5:33abcc31b0aa 25 Timer TXRX_tx_dt_timer;
MatteoT 5:33abcc31b0aa 26 Timer TXRX_rx_dt_timer;
MatteoT 5:33abcc31b0aa 27 char TXRX_buffer [TXRX_BUFFER];
MatteoT 5:33abcc31b0aa 28
MatteoT 5:33abcc31b0aa 29 //Addresses
MatteoT 5:33abcc31b0aa 30 #define DEST_MAC_ADDR 0xF8,0xD1,0x11,0xA0,0x51,0xD0 //MAC address of my TL-MR3020 router
MatteoT 5:33abcc31b0aa 31 #define MY_IP_ADDR 0xc0,0xa8,0x02,0x10
MatteoT 5:33abcc31b0aa 32 #define DEST_IP_ADDR 0xc0,0xa8,0x02,0x01
MatteoT 5:33abcc31b0aa 33 #define UDP_PORT 0x08,0xb1
MatteoT 5:33abcc31b0aa 34 const char dest_mac_addr [6] = {
MatteoT 5:33abcc31b0aa 35 DEST_MAC_ADDR
MatteoT 5:33abcc31b0aa 36 };
MatteoT 5:33abcc31b0aa 37
MatteoT 5:33abcc31b0aa 38 //dumped frames:
MatteoT 5:33abcc31b0aa 39 char ethernet_frame [14] = { //Ethernetv2 packet header
MatteoT 5:33abcc31b0aa 40 0xff,0xff,0xff,0xff,0xff,0xff, //DEST ADDR
MatteoT 5:33abcc31b0aa 41 0x00,0x02,0xf7,0xf1,0xa4,0xdb, //SRC ADDR
MatteoT 5:33abcc31b0aa 42 0x08,0x00 //PROTO
MatteoT 5:33abcc31b0aa 43 };
MatteoT 5:33abcc31b0aa 44 const char ethernet_frame_arp_proto [2] = {
MatteoT 5:33abcc31b0aa 45 0x08,0x06
MatteoT 5:33abcc31b0aa 46 };
MatteoT 5:33abcc31b0aa 47 char ipudp_frame [28] = { //IPv4/UDP packet header (message must be MSG_LEN bytes)
MatteoT 5:33abcc31b0aa 48 0x45, //IP: VER/HEADER LEN
MatteoT 5:33abcc31b0aa 49 0x00, //DIFFERENTIAL SERVICE
MatteoT 5:33abcc31b0aa 50 0x02,0x40, //TOTAL LEN
MatteoT 5:33abcc31b0aa 51 0x01,0xa0, //ID
MatteoT 5:33abcc31b0aa 52 0x00, //FLAGS
MatteoT 5:33abcc31b0aa 53 0x00, //FRAG OFFSET
MatteoT 5:33abcc31b0aa 54 0xff, //TTL
MatteoT 5:33abcc31b0aa 55 0x11, //PROTO=udp
MatteoT 5:33abcc31b0aa 56 0x32,0x99, //HEADER CKS
MatteoT 5:33abcc31b0aa 57 MY_IP_ADDR, //SOURCE ADDR
MatteoT 5:33abcc31b0aa 58 DEST_IP_ADDR, //DEST ADDR
MatteoT 5:33abcc31b0aa 59 //UDP:
MatteoT 5:33abcc31b0aa 60 UDP_PORT, //SOURCE PORT
MatteoT 5:33abcc31b0aa 61 UDP_PORT, //DEST PORT
MatteoT 5:33abcc31b0aa 62 0x02,0x2c, //TOTAL LEN
MatteoT 5:33abcc31b0aa 63 0x27,0xe2 //CKS
MatteoT 5:33abcc31b0aa 64 };
MatteoT 5:33abcc31b0aa 65 char arp_frame [28] = { //Address Resolution Protocol packet
MatteoT 5:33abcc31b0aa 66 0x00,0x01, //HTYPE ethernet
MatteoT 5:33abcc31b0aa 67 0x08,0x00, //PTYPE IP
MatteoT 5:33abcc31b0aa 68 0x06,0x04, //HLEN, PLEN
MatteoT 5:33abcc31b0aa 69 0x00,0x02, //OPER reply
MatteoT 5:33abcc31b0aa 70 0x00,0x02,0xf7,0xf1,0xa4,0xdb, //Sender hw addr
MatteoT 5:33abcc31b0aa 71 MY_IP_ADDR, //Sender proto addr
MatteoT 5:33abcc31b0aa 72 DEST_MAC_ADDR, //Target hw addr
MatteoT 5:33abcc31b0aa 73 DEST_IP_ADDR //Target proto addr
MatteoT 5:33abcc31b0aa 74 };
MatteoT 5:33abcc31b0aa 75
MatteoT 5:33abcc31b0aa 76 void TXRX_thread_routine (void const *args){ //New magic version
MatteoT 5:33abcc31b0aa 77 DigitalOut led_rx(LED_RX); led_rx = 0;
MatteoT 5:33abcc31b0aa 78
MatteoT 5:33abcc31b0aa 79 //Prepare state
MatteoT 5:33abcc31b0aa 80 TXRX_txQuadState.reset();
MatteoT 5:33abcc31b0aa 81 TXRX_rxQuadState.reset();
MatteoT 5:33abcc31b0aa 82
MatteoT 5:33abcc31b0aa 83 //Setup the magic networking
MatteoT 5:33abcc31b0aa 84 Thread::wait(2000);
MatteoT 5:33abcc31b0aa 85 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 86 Ethernet eth0;
MatteoT 5:33abcc31b0aa 87 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 88 eth0.address(ethernet_frame+6);
MatteoT 5:33abcc31b0aa 89 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 90 Thread::wait(2000);
MatteoT 5:33abcc31b0aa 91
MatteoT 5:33abcc31b0aa 92 //infinite loop: if disconnected, reconnects.
MatteoT 5:33abcc31b0aa 93 while(1){
MatteoT 5:33abcc31b0aa 94
MatteoT 5:33abcc31b0aa 95 //Wait eth0 link
MatteoT 5:33abcc31b0aa 96 Thread::wait(10);
MatteoT 5:33abcc31b0aa 97 while(!eth0.link()){
MatteoT 5:33abcc31b0aa 98 SLOW_FLASH_ON(led_rx,1);
MatteoT 5:33abcc31b0aa 99 };
MatteoT 5:33abcc31b0aa 100 Thread::wait(1);
MatteoT 5:33abcc31b0aa 101 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 102
MatteoT 5:33abcc31b0aa 103 //Perform 5 gratuitous ARP reply
MatteoT 5:33abcc31b0aa 104 memset(ethernet_frame, 0xff, 6); //ethernet broadcast
MatteoT 5:33abcc31b0aa 105 memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw addr = my mac; sender ip addr predefined.
MatteoT 5:33abcc31b0aa 106 memcpy(arp_frame+ARP_TARGET_HW_ADDR_OFFSET, arp_frame+ARP_SENDER_HW_ADDR_OFFSET, 6+4); //target = sender (both hw and ip - gratuitous ARP reply as announcement).
MatteoT 5:33abcc31b0aa 107 for(int i=0;i<5;++i){
MatteoT 5:33abcc31b0aa 108 eth0.write(ethernet_frame,12); //ethernet dest and src addr
MatteoT 5:33abcc31b0aa 109 eth0.write(ethernet_frame_arp_proto,2); //ethernet proto field = arp
MatteoT 5:33abcc31b0aa 110 eth0.write(arp_frame,ARP_LEN); //arp packet
MatteoT 5:33abcc31b0aa 111 eth0.send();
MatteoT 5:33abcc31b0aa 112 Thread::wait(10);
MatteoT 5:33abcc31b0aa 113 }
MatteoT 5:33abcc31b0aa 114 FAST_FLASH_ON(led_rx,5);
MatteoT 5:33abcc31b0aa 115 Thread::wait(5);
MatteoT 5:33abcc31b0aa 116
MatteoT 5:33abcc31b0aa 117
MatteoT 5:33abcc31b0aa 118 //Prepare timers
MatteoT 5:33abcc31b0aa 119 TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
MatteoT 5:33abcc31b0aa 120 TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
MatteoT 5:33abcc31b0aa 121
MatteoT 5:33abcc31b0aa 122 //Data to be setted into thread-safe area.
MatteoT 5:33abcc31b0aa 123 float actual_tx_dt, target_tx_dt, actual_rx_dt, target_rx_dt;
MatteoT 5:33abcc31b0aa 124
MatteoT 5:33abcc31b0aa 125 unsigned int step=0;
MatteoT 5:33abcc31b0aa 126 bool accept=false, sent=false, connected=true;
MatteoT 5:33abcc31b0aa 127 while(connected){
MatteoT 5:33abcc31b0aa 128
MatteoT 5:33abcc31b0aa 129 led_rx = (step % 40 < 35 ? 1 : (actual_rx_dt < 1 ? 1 : 0)); //allways light on while receiving, blink 0.5Hz while not receiving.
MatteoT 5:33abcc31b0aa 130
MatteoT 5:33abcc31b0aa 131 { //RECEIVE
MatteoT 5:33abcc31b0aa 132 accept = false;
MatteoT 5:33abcc31b0aa 133 const int received = eth0.receive();
MatteoT 5:33abcc31b0aa 134 if(received > ETH_PAYLOAD_OFFSET && received < TXRX_BUFFER){ //check ethernet frame packet
MatteoT 5:33abcc31b0aa 135 //ACCEPTANCE
MatteoT 5:33abcc31b0aa 136 accept = (eth0.read(TXRX_buffer,received) == received ? true : false);
MatteoT 5:33abcc31b0aa 137 //memcpy(ethernet_frame, TXRX_buffer+6, 6);//sent dest hw addr = received src hw addr
MatteoT 5:33abcc31b0aa 138 //CHECK ARP PACKET AND SEND REPLY
MatteoT 5:33abcc31b0aa 139 if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame_arp_proto,2)){
MatteoT 5:33abcc31b0aa 140 accept = false; //consider arp packets as not accepted.
MatteoT 5:33abcc31b0aa 141 memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw = my mac
MatteoT 5:33abcc31b0aa 142 memcpy(arp_frame+ARP_TARGET_HW_ADDR_OFFSET, TXRX_buffer+ETH_PAYLOAD_OFFSET+ARP_SENDER_HW_ADDR_OFFSET, 6+4); //target = sender (both hw and ip)
MatteoT 5:33abcc31b0aa 143 eth0.write(ethernet_frame,12);
MatteoT 5:33abcc31b0aa 144 eth0.write(ethernet_frame_arp_proto,2);
MatteoT 5:33abcc31b0aa 145 eth0.write(arp_frame,ARP_LEN);
MatteoT 5:33abcc31b0aa 146 eth0.send();
MatteoT 5:33abcc31b0aa 147 }
MatteoT 5:33abcc31b0aa 148 else if(accept && received > ETH_PAYLOAD_OFFSET + IP_UDP_PAYLOAD_OFFSET){
MatteoT 5:33abcc31b0aa 149 //VALIDATE UDP PACKET
MatteoT 5:33abcc31b0aa 150 TXRX_buffer[received] = 0; //string termination
MatteoT 5:33abcc31b0aa 151 if(0 != memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame+ETH_PROTO_OFFSET, 2)) accept = false;
MatteoT 5:33abcc31b0aa 152 if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_PROTO_OFFSET,ipudp_frame+IP_PROTO_OFFSET, 2)) accept = false;
MatteoT 5:33abcc31b0aa 153 if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_DESTPORT_OFFSET,ipudp_frame+IP_UDP_DESTPORT_OFFSET, 2)) accept = false;
MatteoT 5:33abcc31b0aa 154 }
MatteoT 5:33abcc31b0aa 155 else accept = false;
MatteoT 5:33abcc31b0aa 156 }
MatteoT 5:33abcc31b0aa 157 }
MatteoT 5:33abcc31b0aa 158 TXRX_mutex.lock();
MatteoT 5:33abcc31b0aa 159 //PRODUCE ACCEPTING DATA
MatteoT 5:33abcc31b0aa 160 if(accept && QuadState::length() == TXRX_rxQuadState.setFromJSON(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_PAYLOAD_OFFSET))
MatteoT 5:33abcc31b0aa 161 QUAD_STATE_UPDATE_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
MatteoT 5:33abcc31b0aa 162 else
MatteoT 5:33abcc31b0aa 163 QUAD_STATE_READ_ACTUAL_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
MatteoT 5:33abcc31b0aa 164 actual_rx_dt = TXRX_rxQuadState.actual_rx_dt;
MatteoT 5:33abcc31b0aa 165 target_rx_dt = TXRX_rxQuadState.target_rx_dt;
MatteoT 5:33abcc31b0aa 166
MatteoT 5:33abcc31b0aa 167 //TIME MEASURE SENT DATA
MatteoT 5:33abcc31b0aa 168 if(sent)
MatteoT 5:33abcc31b0aa 169 QUAD_STATE_UPDATE_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
MatteoT 5:33abcc31b0aa 170 else
MatteoT 5:33abcc31b0aa 171 QUAD_STATE_READ_ACTUAL_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
MatteoT 5:33abcc31b0aa 172 actual_tx_dt = TXRX_rxQuadState.actual_tx_dt;
MatteoT 5:33abcc31b0aa 173 target_tx_dt = TXRX_rxQuadState.target_tx_dt;
MatteoT 5:33abcc31b0aa 174
MatteoT 5:33abcc31b0aa 175 { //SEND
MatteoT 5:33abcc31b0aa 176 char json [MSG_LEN];
MatteoT 5:33abcc31b0aa 177 memset(json,' ',MSG_LEN);
MatteoT 5:33abcc31b0aa 178 int setted = TXRX_txQuadState.getJSON(json);
MatteoT 5:33abcc31b0aa 179 TXRX_mutex.unlock();
MatteoT 5:33abcc31b0aa 180 if(true || setted == QuadState::length()){
MatteoT 5:33abcc31b0aa 181 //WRITE PACKET AND SEND
MatteoT 5:33abcc31b0aa 182 eth0.write(ethernet_frame,ETH_PAYLOAD_OFFSET);
MatteoT 5:33abcc31b0aa 183 eth0.write(ipudp_frame,IP_UDP_PAYLOAD_OFFSET);
MatteoT 5:33abcc31b0aa 184 eth0.write(json,MSG_LEN);
MatteoT 5:33abcc31b0aa 185 sent = (eth0.send() ? true : false);
MatteoT 5:33abcc31b0aa 186 }
MatteoT 5:33abcc31b0aa 187 }
MatteoT 5:33abcc31b0aa 188
MatteoT 5:33abcc31b0aa 189 led_rx = 0;
MatteoT 5:33abcc31b0aa 190
MatteoT 5:33abcc31b0aa 191 ///REALX TIMINGS
MatteoT 5:33abcc31b0aa 192 //Sleep the shortest time needed to reach a dt_target; also sleep 20ms anyway.
MatteoT 5:33abcc31b0aa 193 double diff_tx = target_tx_dt - actual_tx_dt;
MatteoT 5:33abcc31b0aa 194 //double diff_rx = target_rx_dt - actual_rx_dt;
MatteoT 5:33abcc31b0aa 195 double diff_to_sleep = /*min(diff_rx,*/ diff_tx/*)*/ - 0.020;
MatteoT 5:33abcc31b0aa 196 if(diff_to_sleep > 0)
MatteoT 5:33abcc31b0aa 197 Thread::wait(diff_to_sleep);
MatteoT 5:33abcc31b0aa 198 Thread::wait(20); //NOTE: why?
MatteoT 5:33abcc31b0aa 199
MatteoT 5:33abcc31b0aa 200 if(actual_rx_dt > 1)//check link when receiving nothing
MatteoT 5:33abcc31b0aa 201 connected = eth0.link();
MatteoT 5:33abcc31b0aa 202 ++step;
MatteoT 5:33abcc31b0aa 203 }
MatteoT 5:33abcc31b0aa 204 //end of while(connected)
MatteoT 5:33abcc31b0aa 205 }
MatteoT 5:33abcc31b0aa 206 }
MatteoT 5:33abcc31b0aa 207
MatteoT 5:33abcc31b0aa 208
MatteoT 5:33abcc31b0aa 209 //////////////////////////////////////////////////////
MatteoT 5:33abcc31b0aa 210
MatteoT 5:33abcc31b0aa 211
MatteoT 5:33abcc31b0aa 212 bool TXRX_stateExchange (QuadState & tx, QuadState & rx){
MatteoT 5:33abcc31b0aa 213 if(TXRX_mutex.trylock()){
MatteoT 5:33abcc31b0aa 214 rx = TXRX_rxQuadState;
MatteoT 5:33abcc31b0aa 215 TXRX_txQuadState = tx;
MatteoT 5:33abcc31b0aa 216 TXRX_mutex.unlock();
MatteoT 5:33abcc31b0aa 217 return true;
MatteoT 5:33abcc31b0aa 218 }
MatteoT 5:33abcc31b0aa 219 return false;
MatteoT 5:33abcc31b0aa 220 }
MatteoT 5:33abcc31b0aa 221
MatteoT 5:33abcc31b0aa 222
MatteoT 5:33abcc31b0aa 223 //////////////////////////////////////////////////////
MatteoT 5:33abcc31b0aa 224
MatteoT 5:33abcc31b0aa 225
MatteoT 5:33abcc31b0aa 226
MatteoT 5:33abcc31b0aa 227
MatteoT 5:33abcc31b0aa 228
MatteoT 5:33abcc31b0aa 229
MatteoT 5:33abcc31b0aa 230
MatteoT 5:33abcc31b0aa 231 /*
MatteoT 5:33abcc31b0aa 232
MatteoT 5:33abcc31b0aa 233 //#define TXRX_PORT 2224
MatteoT 5:33abcc31b0aa 234 //#define TXRX_BROADCAST_ADDR "192.168.2.255"
MatteoT 5:33abcc31b0aa 235 //#define TXRX_BROADCAST_PORT 2225
MatteoT 5:33abcc31b0aa 236
MatteoT 5:33abcc31b0aa 237 char TXRX_buffer [TXRX_BUFFER];
MatteoT 5:33abcc31b0aa 238 void TXRX_thread_routine (void const *args){
MatteoT 5:33abcc31b0aa 239 DigitalOut led_rx(LED_RX); led_rx = 0;
MatteoT 5:33abcc31b0aa 240
MatteoT 5:33abcc31b0aa 241
MatteoT 5:33abcc31b0aa 242 //Setup RX socket
MatteoT 5:33abcc31b0aa 243 UDPSocket sock;
MatteoT 5:33abcc31b0aa 244 sock.bind(TXRX_PORT);
MatteoT 5:33abcc31b0aa 245 sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.5);
MatteoT 5:33abcc31b0aa 246 //sock.set_broadcasting();
MatteoT 5:33abcc31b0aa 247 Endpoint server_endpoint;
MatteoT 5:33abcc31b0aa 248
MatteoT 5:33abcc31b0aa 249 Setup broadcast socket
MatteoT 5:33abcc31b0aa 250 UDPSocket response_sock;
MatteoT 5:33abcc31b0aa 251 response_sock.init();
MatteoT 5:33abcc31b0aa 252 response_sock.set_blocking(false, min(TARGET_TX_DT,TARGET_RX_DT)*0.5);
MatteoT 5:33abcc31b0aa 253 response_sock.set_broadcasting();
MatteoT 5:33abcc31b0aa 254 Endpoint broadcast;
MatteoT 5:33abcc31b0aa 255 broadcast.set_address(TXRX_BROADCAST_ADDR, TXRX_BROADCAST_PORT);
MatteoT 5:33abcc31b0aa 256
MatteoT 5:33abcc31b0aa 257
MatteoT 5:33abcc31b0aa 258
MatteoT 5:33abcc31b0aa 259 //Prepare state
MatteoT 5:33abcc31b0aa 260 TXRX_txQuadState.reset();
MatteoT 5:33abcc31b0aa 261 TXRX_rxQuadState.reset();
MatteoT 5:33abcc31b0aa 262
MatteoT 5:33abcc31b0aa 263 //Prepare timers
MatteoT 5:33abcc31b0aa 264 TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
MatteoT 5:33abcc31b0aa 265 TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
MatteoT 5:33abcc31b0aa 266
MatteoT 5:33abcc31b0aa 267
MatteoT 5:33abcc31b0aa 268 //Data to be setted into thread-safe area.
MatteoT 5:33abcc31b0aa 269 double actual_tx_dt, target_tx_dt;
MatteoT 5:33abcc31b0aa 270 int rx_result=-1, tx_result=-1;
MatteoT 5:33abcc31b0aa 271 //Data to be setted from thread-safe area.
MatteoT 5:33abcc31b0aa 272 double actual_rx_dt, target_rx_dt;
MatteoT 5:33abcc31b0aa 273
MatteoT 5:33abcc31b0aa 274
MatteoT 5:33abcc31b0aa 275
MatteoT 5:33abcc31b0aa 276 //Main tx/rx loop
MatteoT 5:33abcc31b0aa 277 //====================================================================
MatteoT 5:33abcc31b0aa 278
MatteoT 5:33abcc31b0aa 279 led_rx = 1; //say it's ready!
MatteoT 5:33abcc31b0aa 280
MatteoT 5:33abcc31b0aa 281 while(true){
MatteoT 5:33abcc31b0aa 282
MatteoT 5:33abcc31b0aa 283
MatteoT 5:33abcc31b0aa 284 //Receive
MatteoT 5:33abcc31b0aa 285 std::fill(TXRX_buffer, TXRX_buffer+TXRX_BUFFER, 0); //zero the buffer
MatteoT 5:33abcc31b0aa 286 rx_result = sock.receiveFrom(server_endpoint, TXRX_buffer, TXRX_BUFFER);
MatteoT 5:33abcc31b0aa 287
MatteoT 5:33abcc31b0aa 288 TXRX_mutex.lock();
MatteoT 5:33abcc31b0aa 289
MatteoT 5:33abcc31b0aa 290 led_rx = 1;
MatteoT 5:33abcc31b0aa 291
MatteoT 5:33abcc31b0aa 292 //check rx time
MatteoT 5:33abcc31b0aa 293 if(rx_result > 0 &&
MatteoT 5:33abcc31b0aa 294 TXRX_rxQuadState.setFromJSON(TXRX_buffer) > 0
MatteoT 5:33abcc31b0aa 295 ){
MatteoT 5:33abcc31b0aa 296 QUAD_STATE_UPDATE_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
MatteoT 5:33abcc31b0aa 297 }else{
MatteoT 5:33abcc31b0aa 298 QUAD_STATE_READ_ACTUAL_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
MatteoT 5:33abcc31b0aa 299 }
MatteoT 5:33abcc31b0aa 300 actual_rx_dt = TXRX_rxQuadState.actual_rx_dt;
MatteoT 5:33abcc31b0aa 301 target_rx_dt = TXRX_rxQuadState.target_rx_dt;
MatteoT 5:33abcc31b0aa 302
MatteoT 5:33abcc31b0aa 303 //check previous tx time
MatteoT 5:33abcc31b0aa 304 if(tx_result > 0){
MatteoT 5:33abcc31b0aa 305 QUAD_STATE_UPDATE_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
MatteoT 5:33abcc31b0aa 306 }else{
MatteoT 5:33abcc31b0aa 307 QUAD_STATE_READ_ACTUAL_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
MatteoT 5:33abcc31b0aa 308 }
MatteoT 5:33abcc31b0aa 309 actual_tx_dt = TXRX_rxQuadState.actual_tx_dt;
MatteoT 5:33abcc31b0aa 310 target_tx_dt = TXRX_rxQuadState.target_tx_dt;
MatteoT 5:33abcc31b0aa 311
MatteoT 5:33abcc31b0aa 312
MatteoT 5:33abcc31b0aa 313 //Prepare broadcast response
MatteoT 5:33abcc31b0aa 314 unsigned int tx_len;
MatteoT 5:33abcc31b0aa 315 {
MatteoT 5:33abcc31b0aa 316 std::string tx_str = "@";TXRX_txQuadState.getJSON();
MatteoT 5:33abcc31b0aa 317
MatteoT 5:33abcc31b0aa 318 TXRX_mutex.unlock();
MatteoT 5:33abcc31b0aa 319
MatteoT 5:33abcc31b0aa 320 tx_str = "\n\n"+tx_str+"\n\0";
MatteoT 5:33abcc31b0aa 321 tx_len = tx_str.length();
MatteoT 5:33abcc31b0aa 322 if(tx_len > TXRX_BUFFER) tx_len = TXRX_BUFFER;
MatteoT 5:33abcc31b0aa 323 memcpy(TXRX_buffer, tx_str.c_str(), tx_len );
MatteoT 5:33abcc31b0aa 324 }
MatteoT 5:33abcc31b0aa 325 //Send broadcast response
MatteoT 5:33abcc31b0aa 326 tx_result = response_sock.sendTo(broadcast,TXRX_buffer,tx_len);
MatteoT 5:33abcc31b0aa 327
MatteoT 5:33abcc31b0aa 328 led_rx = 0;
MatteoT 5:33abcc31b0aa 329
MatteoT 5:33abcc31b0aa 330 //Sleep the shortest time needed to reach a dt_target; also sleep 5ms anyway.
MatteoT 5:33abcc31b0aa 331 double diff_tx = target_tx_dt - actual_tx_dt;
MatteoT 5:33abcc31b0aa 332 double diff_rx = target_rx_dt - actual_rx_dt;
MatteoT 5:33abcc31b0aa 333 double diff_to_sleep = min(diff_rx, diff_tx) - 0.005;
MatteoT 5:33abcc31b0aa 334 QUAD_STATE_WAIT_DT_TARGET(0, diff_to_sleep);
MatteoT 5:33abcc31b0aa 335 Thread::wait(40); //NOTE: why?
MatteoT 5:33abcc31b0aa 336 }
MatteoT 5:33abcc31b0aa 337 }*/
MatteoT 5:33abcc31b0aa 338
MatteoT 5:33abcc31b0aa 339
MatteoT 5:33abcc31b0aa 340
MatteoT 5:33abcc31b0aa 341
MatteoT 5:33abcc31b0aa 342 /*
MatteoT 5:33abcc31b0aa 343 char state = 'N';
MatteoT 5:33abcc31b0aa 344 char name[TXRX_BUFFER]; //N
MatteoT 5:33abcc31b0aa 345 char value[TXRX_BUFFER]; //v
MatteoT 5:33abcc31b0aa 346 unsigned int i=0;
MatteoT 5:33abcc31b0aa 347 unsigned int name_i=0;
MatteoT 5:33abcc31b0aa 348 unsigned int value_i=0;
MatteoT 5:33abcc31b0aa 349
MatteoT 5:33abcc31b0aa 350 bzero(name, TXRX_BUFFER);
MatteoT 5:33abcc31b0aa 351 bzero(value, TXRX_BUFFER);
MatteoT 5:33abcc31b0aa 352
MatteoT 5:33abcc31b0aa 353 TXRX_mutex.lock();
MatteoT 5:33abcc31b0aa 354
MatteoT 5:33abcc31b0aa 355 while(i < TXRX_BUFFER){
MatteoT 5:33abcc31b0aa 356 switch(state){
MatteoT 5:33abcc31b0aa 357 case 'N':
MatteoT 5:33abcc31b0aa 358 if(buffer[i] == ':'){
MatteoT 5:33abcc31b0aa 359 state = 'v';
MatteoT 5:33abcc31b0aa 360 value_i = 0;
MatteoT 5:33abcc31b0aa 361 bzero(value, TXRX_BUFFER);
MatteoT 5:33abcc31b0aa 362 }
MatteoT 5:33abcc31b0aa 363 else
MatteoT 5:33abcc31b0aa 364 name[name_i++] = buffer[i];
MatteoT 5:33abcc31b0aa 365 break;
MatteoT 5:33abcc31b0aa 366 case 'v':
MatteoT 5:33abcc31b0aa 367 if(buffer[i] == '\r' || buffer[i] == '\n'){
MatteoT 5:33abcc31b0aa 368 //End of name:value pair
MatteoT 5:33abcc31b0aa 369 void* var = TXRX_rxQuadState[name];
MatteoT 5:33abcc31b0aa 370 if(var != NULL){
MatteoT 5:33abcc31b0aa 371 *var =
MatteoT 5:33abcc31b0aa 372 }
MatteoT 5:33abcc31b0aa 373
MatteoT 5:33abcc31b0aa 374 //prepare for next pair
MatteoT 5:33abcc31b0aa 375 state = 'N';
MatteoT 5:33abcc31b0aa 376 name_i = 0;
MatteoT 5:33abcc31b0aa 377 bzero(name, TXRX_BUFFER);
MatteoT 5:33abcc31b0aa 378 }
MatteoT 5:33abcc31b0aa 379 else
MatteoT 5:33abcc31b0aa 380 value[value_i++] = buffer[i];
MatteoT 5:33abcc31b0aa 381 break;
MatteoT 5:33abcc31b0aa 382 }
MatteoT 5:33abcc31b0aa 383 ++i;
MatteoT 5:33abcc31b0aa 384 }*/