Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

TXRX_magic.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?

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 6:4ddd68260f76 10 #define IP_CKS_OFFSET (10)
MatteoT 5:33abcc31b0aa 11 #define IP_SRC_ADDR_OFFSET (12)
MatteoT 5:33abcc31b0aa 12 #define IP_DEST_ADDR_OFFSET (16)
MatteoT 5:33abcc31b0aa 13 #define IP_PAYLOAD_OFFSET (20)
MatteoT 5:33abcc31b0aa 14 #define IP_UDP_DESTPORT_OFFSET (20+2)
MatteoT 5:33abcc31b0aa 15 #define IP_UDP_LEN_OFFSET (20+2+2)
MatteoT 6:4ddd68260f76 16 #define IP_UDP_CKS_OFFSET (20+2+2+2)
MatteoT 5:33abcc31b0aa 17 #define IP_UDP_PAYLOAD_OFFSET (20+8)
MatteoT 5:33abcc31b0aa 18 //ARP:
MatteoT 5:33abcc31b0aa 19 #define ARP_SENDER_HW_ADDR_OFFSET (8)
MatteoT 5:33abcc31b0aa 20 #define ARP_TARGET_HW_ADDR_OFFSET (8+6+4)
MatteoT 5:33abcc31b0aa 21 #define ARP_LEN (28)
MatteoT 5:33abcc31b0aa 22
MatteoT 5:33abcc31b0aa 23 /////////////////////////////////////////////////////////////////
MatteoT 6:4ddd68260f76 24 struct QuadStateTXRXTimes{
MatteoT 6:4ddd68260f76 25 double target_tx_dt;
MatteoT 6:4ddd68260f76 26 double actual_tx_dt;
MatteoT 6:4ddd68260f76 27 double average_tx_dt;
MatteoT 6:4ddd68260f76 28 double average_tx_dt_k;
MatteoT 6:4ddd68260f76 29
MatteoT 6:4ddd68260f76 30 double target_rx_dt;
MatteoT 6:4ddd68260f76 31 double actual_rx_dt;
MatteoT 6:4ddd68260f76 32 double average_rx_dt;
MatteoT 6:4ddd68260f76 33 double average_rx_dt_k;
MatteoT 6:4ddd68260f76 34
MatteoT 6:4ddd68260f76 35 void reset(){
MatteoT 6:4ddd68260f76 36 target_tx_dt = TARGET_TX_DT;
MatteoT 6:4ddd68260f76 37 actual_tx_dt = TARGET_TX_DT;
MatteoT 6:4ddd68260f76 38 average_tx_dt = TARGET_TX_DT;
MatteoT 6:4ddd68260f76 39 average_tx_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 6:4ddd68260f76 40
MatteoT 6:4ddd68260f76 41 target_rx_dt = TARGET_RX_DT;
MatteoT 6:4ddd68260f76 42 actual_rx_dt = TARGET_RX_DT;
MatteoT 6:4ddd68260f76 43 average_rx_dt = TARGET_RX_DT;
MatteoT 6:4ddd68260f76 44 average_rx_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 6:4ddd68260f76 45 }
MatteoT 6:4ddd68260f76 46 };
MatteoT 6:4ddd68260f76 47 QuadStateTXRXTimes TXRX_times;
MatteoT 5:33abcc31b0aa 48 Mutex TXRX_mutex;
MatteoT 5:33abcc31b0aa 49 QuadState TXRX_txQuadState;
MatteoT 5:33abcc31b0aa 50 QuadState TXRX_rxQuadState;
MatteoT 5:33abcc31b0aa 51 Timer TXRX_tx_dt_timer;
MatteoT 5:33abcc31b0aa 52 Timer TXRX_rx_dt_timer;
MatteoT 5:33abcc31b0aa 53 char TXRX_buffer [TXRX_BUFFER];
MatteoT 5:33abcc31b0aa 54
MatteoT 5:33abcc31b0aa 55 //Addresses
MatteoT 5:33abcc31b0aa 56 #define DEST_MAC_ADDR 0xF8,0xD1,0x11,0xA0,0x51,0xD0 //MAC address of my TL-MR3020 router
MatteoT 5:33abcc31b0aa 57 #define MY_IP_ADDR 0xc0,0xa8,0x02,0x10
MatteoT 5:33abcc31b0aa 58 #define DEST_IP_ADDR 0xc0,0xa8,0x02,0x01
MatteoT 5:33abcc31b0aa 59 #define UDP_PORT 0x08,0xb1
MatteoT 5:33abcc31b0aa 60 const char dest_mac_addr [6] = {
MatteoT 5:33abcc31b0aa 61 DEST_MAC_ADDR
MatteoT 5:33abcc31b0aa 62 };
MatteoT 5:33abcc31b0aa 63
MatteoT 5:33abcc31b0aa 64 //dumped frames:
MatteoT 5:33abcc31b0aa 65 char ethernet_frame [14] = { //Ethernetv2 packet header
MatteoT 5:33abcc31b0aa 66 0xff,0xff,0xff,0xff,0xff,0xff, //DEST ADDR
MatteoT 5:33abcc31b0aa 67 0x00,0x02,0xf7,0xf1,0xa4,0xdb, //SRC ADDR
MatteoT 5:33abcc31b0aa 68 0x08,0x00 //PROTO
MatteoT 5:33abcc31b0aa 69 };
MatteoT 5:33abcc31b0aa 70 const char ethernet_frame_arp_proto [2] = {
MatteoT 5:33abcc31b0aa 71 0x08,0x06
MatteoT 5:33abcc31b0aa 72 };
MatteoT 5:33abcc31b0aa 73 char ipudp_frame [28] = { //IPv4/UDP packet header (message must be MSG_LEN bytes)
MatteoT 5:33abcc31b0aa 74 0x45, //IP: VER/HEADER LEN
MatteoT 5:33abcc31b0aa 75 0x00, //DIFFERENTIAL SERVICE
MatteoT 5:33abcc31b0aa 76 0x02,0x40, //TOTAL LEN
MatteoT 5:33abcc31b0aa 77 0x01,0xa0, //ID
MatteoT 5:33abcc31b0aa 78 0x00, //FLAGS
MatteoT 5:33abcc31b0aa 79 0x00, //FRAG OFFSET
MatteoT 5:33abcc31b0aa 80 0xff, //TTL
MatteoT 5:33abcc31b0aa 81 0x11, //PROTO=udp
MatteoT 5:33abcc31b0aa 82 0x32,0x99, //HEADER CKS
MatteoT 5:33abcc31b0aa 83 MY_IP_ADDR, //SOURCE ADDR
MatteoT 5:33abcc31b0aa 84 DEST_IP_ADDR, //DEST ADDR
MatteoT 5:33abcc31b0aa 85 //UDP:
MatteoT 5:33abcc31b0aa 86 UDP_PORT, //SOURCE PORT
MatteoT 5:33abcc31b0aa 87 UDP_PORT, //DEST PORT
MatteoT 5:33abcc31b0aa 88 0x02,0x2c, //TOTAL LEN
MatteoT 5:33abcc31b0aa 89 0x27,0xe2 //CKS
MatteoT 5:33abcc31b0aa 90 };
MatteoT 5:33abcc31b0aa 91 char arp_frame [28] = { //Address Resolution Protocol packet
MatteoT 5:33abcc31b0aa 92 0x00,0x01, //HTYPE ethernet
MatteoT 5:33abcc31b0aa 93 0x08,0x00, //PTYPE IP
MatteoT 5:33abcc31b0aa 94 0x06,0x04, //HLEN, PLEN
MatteoT 5:33abcc31b0aa 95 0x00,0x02, //OPER reply
MatteoT 5:33abcc31b0aa 96 0x00,0x02,0xf7,0xf1,0xa4,0xdb, //Sender hw addr
MatteoT 5:33abcc31b0aa 97 MY_IP_ADDR, //Sender proto addr
MatteoT 5:33abcc31b0aa 98 DEST_MAC_ADDR, //Target hw addr
MatteoT 5:33abcc31b0aa 99 DEST_IP_ADDR //Target proto addr
MatteoT 5:33abcc31b0aa 100 };
MatteoT 5:33abcc31b0aa 101
MatteoT 6:4ddd68260f76 102 #define htons(n) ( (((n) & 0xFF00) >> 8) | (((n) & 0x00FF) << 8) )
MatteoT 6:4ddd68260f76 103 uint16_t ip_checksum (const char * buf, size_t hdr_len);
MatteoT 6:4ddd68260f76 104 uint16_t udp_checksum(const char * buff, size_t len, const char * src_addr, const char * dest_addr);
MatteoT 6:4ddd68260f76 105
MatteoT 5:33abcc31b0aa 106 void TXRX_thread_routine (void const *args){ //New magic version
MatteoT 5:33abcc31b0aa 107 DigitalOut led_rx(LED_RX); led_rx = 0;
MatteoT 5:33abcc31b0aa 108
MatteoT 5:33abcc31b0aa 109 //Prepare state
MatteoT 6:4ddd68260f76 110 TXRX_times.reset();
MatteoT 5:33abcc31b0aa 111 TXRX_txQuadState.reset();
MatteoT 5:33abcc31b0aa 112 TXRX_rxQuadState.reset();
MatteoT 5:33abcc31b0aa 113
MatteoT 5:33abcc31b0aa 114 //Setup the magic networking
MatteoT 5:33abcc31b0aa 115 Thread::wait(2000);
MatteoT 5:33abcc31b0aa 116 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 117 Ethernet eth0;
MatteoT 5:33abcc31b0aa 118 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 119 eth0.address(ethernet_frame+6);
MatteoT 5:33abcc31b0aa 120 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 121 Thread::wait(2000);
MatteoT 5:33abcc31b0aa 122
MatteoT 5:33abcc31b0aa 123 //infinite loop: if disconnected, reconnects.
MatteoT 5:33abcc31b0aa 124 while(1){
MatteoT 5:33abcc31b0aa 125
MatteoT 5:33abcc31b0aa 126 //Wait eth0 link
MatteoT 5:33abcc31b0aa 127 Thread::wait(10);
MatteoT 5:33abcc31b0aa 128 while(!eth0.link()){
MatteoT 5:33abcc31b0aa 129 SLOW_FLASH_ON(led_rx,1);
MatteoT 5:33abcc31b0aa 130 };
MatteoT 6:4ddd68260f76 131 Thread::wait(5);
MatteoT 5:33abcc31b0aa 132 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 133
MatteoT 6:4ddd68260f76 134 //Perform 5 gratuitous ARP reply, with an interval of 10ms
MatteoT 5:33abcc31b0aa 135 memset(ethernet_frame, 0xff, 6); //ethernet broadcast
MatteoT 5:33abcc31b0aa 136 memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw addr = my mac; sender ip addr predefined.
MatteoT 5:33abcc31b0aa 137 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 138 for(int i=0;i<5;++i){
MatteoT 5:33abcc31b0aa 139 eth0.write(ethernet_frame,12); //ethernet dest and src addr
MatteoT 5:33abcc31b0aa 140 eth0.write(ethernet_frame_arp_proto,2); //ethernet proto field = arp
MatteoT 5:33abcc31b0aa 141 eth0.write(arp_frame,ARP_LEN); //arp packet
MatteoT 5:33abcc31b0aa 142 eth0.send();
MatteoT 5:33abcc31b0aa 143 Thread::wait(10);
MatteoT 5:33abcc31b0aa 144 }
MatteoT 5:33abcc31b0aa 145 FAST_FLASH_ON(led_rx,5);
MatteoT 5:33abcc31b0aa 146 Thread::wait(5);
MatteoT 5:33abcc31b0aa 147
MatteoT 5:33abcc31b0aa 148
MatteoT 5:33abcc31b0aa 149 //Prepare timers
MatteoT 5:33abcc31b0aa 150 TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
MatteoT 5:33abcc31b0aa 151 TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
MatteoT 5:33abcc31b0aa 152
MatteoT 5:33abcc31b0aa 153 unsigned int step=0;
MatteoT 5:33abcc31b0aa 154 bool accept=false, sent=false, connected=true;
MatteoT 5:33abcc31b0aa 155 while(connected){
MatteoT 5:33abcc31b0aa 156
MatteoT 6:4ddd68260f76 157 led_rx = (step % 40 < 35 ? 1 : (TXRX_times.actual_rx_dt < 1 ? 1 : 0)); //allways light on while receiving, blink 0.5Hz while not receiving.
MatteoT 5:33abcc31b0aa 158
MatteoT 5:33abcc31b0aa 159 { //RECEIVE
MatteoT 5:33abcc31b0aa 160 accept = false;
MatteoT 5:33abcc31b0aa 161 const int received = eth0.receive();
MatteoT 5:33abcc31b0aa 162 if(received > ETH_PAYLOAD_OFFSET && received < TXRX_BUFFER){ //check ethernet frame packet
MatteoT 5:33abcc31b0aa 163 //ACCEPTANCE
MatteoT 6:4ddd68260f76 164 accept = (eth0.read(TXRX_buffer,received) > ETH_PAYLOAD_OFFSET ? true : false);
MatteoT 5:33abcc31b0aa 165 //memcpy(ethernet_frame, TXRX_buffer+6, 6);//sent dest hw addr = received src hw addr
MatteoT 5:33abcc31b0aa 166 //CHECK ARP PACKET AND SEND REPLY
MatteoT 5:33abcc31b0aa 167 if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame_arp_proto,2)){
MatteoT 6:4ddd68260f76 168 accept = false; //consider arp packets as not accepted and reply now.
MatteoT 5:33abcc31b0aa 169 memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw = my mac
MatteoT 5:33abcc31b0aa 170 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 171 eth0.write(ethernet_frame,12);
MatteoT 5:33abcc31b0aa 172 eth0.write(ethernet_frame_arp_proto,2);
MatteoT 5:33abcc31b0aa 173 eth0.write(arp_frame,ARP_LEN);
MatteoT 5:33abcc31b0aa 174 eth0.send();
MatteoT 5:33abcc31b0aa 175 }
MatteoT 6:4ddd68260f76 176 else if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET, ethernet_frame+ETH_PROTO_OFFSET,2)
MatteoT 6:4ddd68260f76 177 && received > ETH_PAYLOAD_OFFSET + IP_UDP_PAYLOAD_OFFSET + 2){
MatteoT 5:33abcc31b0aa 178 //VALIDATE UDP PACKET
MatteoT 5:33abcc31b0aa 179 TXRX_buffer[received] = 0; //string termination
MatteoT 6:4ddd68260f76 180 accept = true;/*
MatteoT 5:33abcc31b0aa 181 if(0 != memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame+ETH_PROTO_OFFSET, 2)) accept = false;
MatteoT 5:33abcc31b0aa 182 if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_PROTO_OFFSET,ipudp_frame+IP_PROTO_OFFSET, 2)) accept = false;
MatteoT 6:4ddd68260f76 183 if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_DESTPORT_OFFSET,ipudp_frame+IP_UDP_DESTPORT_OFFSET, 2)) accept = false;*/
MatteoT 5:33abcc31b0aa 184 }
MatteoT 5:33abcc31b0aa 185 else accept = false;
MatteoT 5:33abcc31b0aa 186 }
MatteoT 5:33abcc31b0aa 187 }
MatteoT 5:33abcc31b0aa 188 TXRX_mutex.lock();
MatteoT 5:33abcc31b0aa 189 //PRODUCE ACCEPTING DATA
MatteoT 6:4ddd68260f76 190 if(accept)
MatteoT 6:4ddd68260f76 191 accept = (QuadState::length() == TXRX_rxQuadState.setFromJSON(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_PAYLOAD_OFFSET));
MatteoT 6:4ddd68260f76 192
MatteoT 6:4ddd68260f76 193 //Import tx/rx times config from main thread
MatteoT 6:4ddd68260f76 194 TXRX_times.target_rx_dt = TXRX_txQuadState.target_rx_dt;
MatteoT 6:4ddd68260f76 195 TXRX_times.average_rx_dt_k = TXRX_txQuadState.average_rx_dt_k;
MatteoT 6:4ddd68260f76 196 TXRX_times.target_tx_dt = TXRX_txQuadState.target_tx_dt;
MatteoT 6:4ddd68260f76 197 TXRX_times.average_tx_dt_k = TXRX_txQuadState.average_tx_dt_k;
MatteoT 6:4ddd68260f76 198 //Export tx/rx times measured to remote and main thread
MatteoT 6:4ddd68260f76 199 TXRX_txQuadState.actual_rx_dt = (TXRX_rxQuadState.actual_rx_dt = TXRX_times.actual_rx_dt);
MatteoT 6:4ddd68260f76 200 TXRX_txQuadState.average_rx_dt = (TXRX_rxQuadState.average_rx_dt = TXRX_times.average_rx_dt);
MatteoT 6:4ddd68260f76 201 TXRX_txQuadState.actual_tx_dt = (TXRX_rxQuadState.actual_tx_dt = TXRX_times.actual_tx_dt);
MatteoT 6:4ddd68260f76 202 TXRX_txQuadState.average_tx_dt = (TXRX_rxQuadState.average_tx_dt = TXRX_times.average_tx_dt);
MatteoT 5:33abcc31b0aa 203
MatteoT 5:33abcc31b0aa 204 { //SEND
MatteoT 6:4ddd68260f76 205 //TXRX_buffer will contain: IP header, UDP header, json message. (no ethernet header).
MatteoT 6:4ddd68260f76 206 //Why? the function udp_checksum needs the udp payload to be next to the udp header.
MatteoT 6:4ddd68260f76 207 memset(TXRX_buffer+IP_UDP_PAYLOAD_OFFSET,' ',MSG_LEN);
MatteoT 6:4ddd68260f76 208 int setted = TXRX_txQuadState.getJSON(TXRX_buffer+IP_UDP_PAYLOAD_OFFSET);
MatteoT 5:33abcc31b0aa 209 TXRX_mutex.unlock();
MatteoT 6:4ddd68260f76 210 //CALC CHECKSUMs
MatteoT 6:4ddd68260f76 211 memcpy(TXRX_buffer,ipudp_frame,IP_UDP_PAYLOAD_OFFSET);
MatteoT 6:4ddd68260f76 212 memset(TXRX_buffer+IP_CKS_OFFSET,0,2);
MatteoT 6:4ddd68260f76 213 memset(TXRX_buffer+IP_UDP_CKS_OFFSET,0,2);
MatteoT 6:4ddd68260f76 214 uint16_t ip_cks = htons ( ip_checksum(TXRX_buffer,IP_PAYLOAD_OFFSET) );
MatteoT 6:4ddd68260f76 215 //uint16_t udp_cks = htons ( udp_checksum(TXRX_buffer+IP_PAYLOAD_OFFSET, 8+MSG_LEN, TXRX_buffer+IP_SRC_ADDR_OFFSET, TXRX_buffer+IP_DEST_ADDR_OFFSET) );
MatteoT 6:4ddd68260f76 216 memcpy(TXRX_buffer+IP_CKS_OFFSET,&ip_cks,2);
MatteoT 6:4ddd68260f76 217 //memcpy(TXRX_buffer+IP_UDP_CKS_OFFSET,&udp_cks,2);
MatteoT 6:4ddd68260f76 218 //WRITE PACKET AND SEND
MatteoT 6:4ddd68260f76 219 eth0.write(ethernet_frame,ETH_PAYLOAD_OFFSET);
MatteoT 6:4ddd68260f76 220 eth0.write(TXRX_buffer,IP_UDP_PAYLOAD_OFFSET+MSG_LEN);
MatteoT 6:4ddd68260f76 221 sent = (eth0.send() ? true : false);
MatteoT 5:33abcc31b0aa 222 }
MatteoT 5:33abcc31b0aa 223
MatteoT 6:4ddd68260f76 224 //check rx/tx dt time
MatteoT 6:4ddd68260f76 225 if(accept)
MatteoT 6:4ddd68260f76 226 QUAD_STATE_UPDATE_DT (TXRX_times, rx, TXRX_rx_dt_timer)
MatteoT 6:4ddd68260f76 227 else
MatteoT 6:4ddd68260f76 228 QUAD_STATE_READ_ACTUAL_DT (TXRX_times, rx, TXRX_rx_dt_timer)
MatteoT 6:4ddd68260f76 229 if(sent)
MatteoT 6:4ddd68260f76 230 QUAD_STATE_UPDATE_DT (TXRX_times, tx, TXRX_tx_dt_timer)
MatteoT 6:4ddd68260f76 231 else
MatteoT 6:4ddd68260f76 232 QUAD_STATE_READ_ACTUAL_DT (TXRX_times, tx, TXRX_tx_dt_timer)
MatteoT 6:4ddd68260f76 233
MatteoT 6:4ddd68260f76 234
MatteoT 6:4ddd68260f76 235 if(TXRX_times.actual_rx_dt > 10.0*TXRX_times.target_rx_dt)//disconnect when receiving nothing for a long time
MatteoT 6:4ddd68260f76 236 connected = false;
MatteoT 6:4ddd68260f76 237 if(TXRX_times.actual_rx_dt > 3.00*TXRX_times.target_rx_dt)//check link when receiving nothing for just a while
MatteoT 6:4ddd68260f76 238 connected = eth0.link();
MatteoT 6:4ddd68260f76 239
MatteoT 5:33abcc31b0aa 240 led_rx = 0;
MatteoT 5:33abcc31b0aa 241
MatteoT 6:4ddd68260f76 242 ///REALX TIMINGS (always sleep at least 48ms)
MatteoT 6:4ddd68260f76 243 double to_sleep = (TXRX_times.target_tx_dt - TXRX_times.actual_tx_dt) - 0.048;
MatteoT 6:4ddd68260f76 244 if(to_sleep < 0)
MatteoT 6:4ddd68260f76 245 to_sleep=0;
MatteoT 6:4ddd68260f76 246 QUAD_STATE_WAIT_DT_TARGET(0, to_sleep)
MatteoT 6:4ddd68260f76 247 Thread::wait(48);
MatteoT 5:33abcc31b0aa 248
MatteoT 5:33abcc31b0aa 249 ++step;
MatteoT 5:33abcc31b0aa 250 }
MatteoT 5:33abcc31b0aa 251 //end of while(connected)
MatteoT 5:33abcc31b0aa 252 }
MatteoT 5:33abcc31b0aa 253 }
MatteoT 5:33abcc31b0aa 254
MatteoT 5:33abcc31b0aa 255
MatteoT 5:33abcc31b0aa 256 //////////////////////////////////////////////////////
MatteoT 5:33abcc31b0aa 257
MatteoT 5:33abcc31b0aa 258
MatteoT 5:33abcc31b0aa 259 bool TXRX_stateExchange (QuadState & tx, QuadState & rx){
MatteoT 5:33abcc31b0aa 260 if(TXRX_mutex.trylock()){
MatteoT 5:33abcc31b0aa 261 rx = TXRX_rxQuadState;
MatteoT 5:33abcc31b0aa 262 TXRX_txQuadState = tx;
MatteoT 5:33abcc31b0aa 263 TXRX_mutex.unlock();
MatteoT 5:33abcc31b0aa 264 return true;
MatteoT 5:33abcc31b0aa 265 }
MatteoT 5:33abcc31b0aa 266 return false;
MatteoT 5:33abcc31b0aa 267 }
MatteoT 5:33abcc31b0aa 268
MatteoT 5:33abcc31b0aa 269
MatteoT 5:33abcc31b0aa 270 //////////////////////////////////////////////////////
MatteoT 5:33abcc31b0aa 271
MatteoT 5:33abcc31b0aa 272
MatteoT 5:33abcc31b0aa 273
MatteoT 5:33abcc31b0aa 274
MatteoT 6:4ddd68260f76 275 uint16_t ip_checksum (const char * buf, size_t hdr_len)
MatteoT 6:4ddd68260f76 276 {
MatteoT 6:4ddd68260f76 277 unsigned long sum = 0;
MatteoT 6:4ddd68260f76 278 const char *ip1 = buf;
MatteoT 6:4ddd68260f76 279 while (hdr_len > 1)
MatteoT 6:4ddd68260f76 280 {
MatteoT 6:4ddd68260f76 281 sum += (*ip1++)<<8; sum += (*ip1++);
MatteoT 6:4ddd68260f76 282 if (sum & 0x80000000)
MatteoT 6:4ddd68260f76 283 sum = (sum & 0xFFFF) + (sum >> 16);
MatteoT 6:4ddd68260f76 284 hdr_len -= 2;
MatteoT 6:4ddd68260f76 285 }
MatteoT 6:4ddd68260f76 286 while (sum >> 16)
MatteoT 6:4ddd68260f76 287 sum = (sum & 0xFFFF) + (sum >> 16);
MatteoT 6:4ddd68260f76 288 return(~sum);
MatteoT 6:4ddd68260f76 289 }
MatteoT 5:33abcc31b0aa 290
MatteoT 6:4ddd68260f76 291 uint16_t udp_checksum(const char * buff, size_t len, const char * src_addr, const char * dest_addr){
MatteoT 6:4ddd68260f76 292 const char *buf=buff;
MatteoT 6:4ddd68260f76 293 uint32_t sum;
MatteoT 6:4ddd68260f76 294 size_t length=len;
MatteoT 6:4ddd68260f76 295 sum = 0;
MatteoT 6:4ddd68260f76 296 while (len > 1){
MatteoT 6:4ddd68260f76 297 sum += (*buf++)<<8; sum += (*buf++);
MatteoT 6:4ddd68260f76 298 if (sum & 0x80000000)
MatteoT 6:4ddd68260f76 299 sum = (sum & 0xFFFF) + (sum >> 16);
MatteoT 6:4ddd68260f76 300 len -= 2;
MatteoT 5:33abcc31b0aa 301 }
MatteoT 6:4ddd68260f76 302 if ( len & 1 )
MatteoT 6:4ddd68260f76 303 sum += *((uint8_t *)buf);
MatteoT 6:4ddd68260f76 304 sum += (*src_addr++)<<8; sum += (*src_addr++);
MatteoT 6:4ddd68260f76 305 sum += (*src_addr++)<<8; sum += (*src_addr++);
MatteoT 6:4ddd68260f76 306 sum += (*dest_addr++)<<8; sum += (*dest_addr++);
MatteoT 6:4ddd68260f76 307 sum += (*dest_addr++)<<8; sum += (*dest_addr++);
MatteoT 6:4ddd68260f76 308 sum += htons(17); //IPPROTO_UDP
MatteoT 6:4ddd68260f76 309 sum += htons(length);
MatteoT 6:4ddd68260f76 310 while (sum >> 16)
MatteoT 6:4ddd68260f76 311 sum = (sum & 0xFFFF) + (sum >> 16);
MatteoT 6:4ddd68260f76 312 return ((uint16_t)(~sum));
MatteoT 6:4ddd68260f76 313 }