Emaxx Navigation Group / decawave_networking

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

decawave_network.cpp@0:bd42ff91c25b, 2017-02-15 (annotated)

Committer:
jdawkins
Date:
Wed Feb 15 17:23:19 2017 +0000
Revision:
0:bd42ff91c25b
Child:
1:93fcc351837a
Creation of decawave networking library that does ranging as well as passing of data between nodes using mavlink message structure. Currently requires mavlink_emaxx library as well;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:bd42ff91c25b 1 #include "decawave_network.h"
jdawkins 0:bd42ff91c25b 2
jdawkins 0:bd42ff91c25b 3 DecaWaveNetwork::DecaWaveNetwork(DW1000& DW) : dw(DW)
jdawkins 0:bd42ff91c25b 4 {
jdawkins 0:bd42ff91c25b 5 isAnchor = true;
jdawkins 0:bd42ff91c25b 6 overflow = false;
jdawkins 0:bd42ff91c25b 7 address = 0;
jdawkins 0:bd42ff91c25b 8 rxTimestamp = 0;
jdawkins 0:bd42ff91c25b 9 timediffRec = 0;
jdawkins 0:bd42ff91c25b 10 timediffSend = 0;
jdawkins 0:bd42ff91c25b 11 for (int i = 0; i < MAX_NODES; i++)
jdawkins 0:bd42ff91c25b 12 acknowledgement[i] = true;
jdawkins 0:bd42ff91c25b 13
jdawkins 0:bd42ff91c25b 14 dw.setCallbacks(this, &DecaWaveNetwork::callbackRX, &DecaWaveNetwork::callbackTX);
jdawkins 0:bd42ff91c25b 15
jdawkins 0:bd42ff91c25b 16 LocalTimer.start();
jdawkins 0:bd42ff91c25b 17 checkConn.attach(this,&DecaWaveNetwork::checkConnectivity, 10.0);
jdawkins 0:bd42ff91c25b 18 dw.startRX();
jdawkins 0:bd42ff91c25b 19 }
jdawkins 0:bd42ff91c25b 20
jdawkins 0:bd42ff91c25b 21 void DecaWaveNetwork::callbackRX()
jdawkins 0:bd42ff91c25b 22 {
jdawkins 0:bd42ff91c25b 23 int n = dw.getFramelength();
jdawkins 0:bd42ff91c25b 24 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, n);
jdawkins 0:bd42ff91c25b 25
jdawkins 0:bd42ff91c25b 26
jdawkins 0:bd42ff91c25b 27
jdawkins 0:bd42ff91c25b 28 if (receivedFrame.destination == address){
jdawkins 0:bd42ff91c25b 29 switch (receivedFrame.type) {
jdawkins 0:bd42ff91c25b 30 case PING:
jdawkins 0:bd42ff91c25b 31 rxTimestamp = dw.getRXTimestamp();
jdawkins 0:bd42ff91c25b 32 receiverTimestamps[receivedFrame.source][0] = rxTimestamp; //Save the first timestamp on the receiving node/anchor (T_rp)
jdawkins 0:bd42ff91c25b 33 sendDelayedAnswer(receivedFrame.source, ANCHOR_RESPONSE, rxTimestamp);
jdawkins 0:bd42ff91c25b 34 break;
jdawkins 0:bd42ff91c25b 35 case ANCHOR_RESPONSE:
jdawkins 0:bd42ff91c25b 36 rxTimestamp = dw.getRXTimestamp();
jdawkins 0:bd42ff91c25b 37 senderTimestamps[receivedFrame.source][1] = rxTimestamp; //Save the second timestamp on the sending node/beacon (T_rr)
jdawkins 0:bd42ff91c25b 38 sendDelayedAnswer(receivedFrame.source, 3, rxTimestamp);
jdawkins 0:bd42ff91c25b 39 break;
jdawkins 0:bd42ff91c25b 40 case BEACON_RESPONSE:
jdawkins 0:bd42ff91c25b 41 rxTimestamp = dw.getRXTimestamp();
jdawkins 0:bd42ff91c25b 42 receiverTimestamps[receivedFrame.source][2] = rxTimestamp; //Save the third timestamp on the receiving node/anchor (T_rf)
jdawkins 0:bd42ff91c25b 43
jdawkins 0:bd42ff91c25b 44 correctReceiverTimestamps(receivedFrame.source); //Correct the timestamps for the case of a counter overflow
jdawkins 0:bd42ff91c25b 45 //calculation of the summand on the receiving node/anchor
jdawkins 0:bd42ff91c25b 46 timediffRec = - 2*receiverTimestamps[receivedFrame.source][1] + receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2];
jdawkins 0:bd42ff91c25b 47 sendTransferFrame(receivedFrame.source, timediffRec );
jdawkins 0:bd42ff91c25b 48 break;
jdawkins 0:bd42ff91c25b 49 case TRANSFER_FRAME:
jdawkins 0:bd42ff91c25b 50 //calculation of the summand on the sending node/beacon
jdawkins 0:bd42ff91c25b 51 timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
jdawkins 0:bd42ff91c25b 52 //calculation of the resulting sum of all four ToFs.
jdawkins 0:bd42ff91c25b 53 tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
jdawkins 0:bd42ff91c25b 54 acknowledgement[receivedFrame.source] = true;
jdawkins 0:bd42ff91c25b 55 break;
jdawkins 0:bd42ff91c25b 56 default :
jdawkins 0:bd42ff91c25b 57 break;
jdawkins 0:bd42ff91c25b 58 }
jdawkins 0:bd42ff91c25b 59 } else{
jdawkins 0:bd42ff91c25b 60
jdawkins 0:bd42ff91c25b 61 memcpy(&mavlink_buffer,&receivedFrame,n);
jdawkins 0:bd42ff91c25b 62 // int i;
jdawkins 0:bd42ff91c25b 63
jdawkins 0:bd42ff91c25b 64 /* for(int i=0; i<n; i++) {
jdawkins 0:bd42ff91c25b 65 printf("%X",mavlink_buffer[i]);
jdawkins 0:bd42ff91c25b 66 }
jdawkins 0:bd42ff91c25b 67 printf("\r\n");*/
jdawkins 0:bd42ff91c25b 68 } // end else
jdawkins 0:bd42ff91c25b 69
jdawkins 0:bd42ff91c25b 70
jdawkins 0:bd42ff91c25b 71 parseMavlinkMsg(n);
jdawkins 0:bd42ff91c25b 72
jdawkins 0:bd42ff91c25b 73 dw.startRX();
jdawkins 0:bd42ff91c25b 74 }
jdawkins 0:bd42ff91c25b 75
jdawkins 0:bd42ff91c25b 76 void DecaWaveNetwork::callbackTX()
jdawkins 0:bd42ff91c25b 77 {
jdawkins 0:bd42ff91c25b 78 switch (rangingFrame.type) {
jdawkins 0:bd42ff91c25b 79 case PING:
jdawkins 0:bd42ff91c25b 80 senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp(); //Save the first timestamp on the sending node/beacon (T_sp)
jdawkins 0:bd42ff91c25b 81 break;
jdawkins 0:bd42ff91c25b 82 case ANCHOR_RESPONSE:
jdawkins 0:bd42ff91c25b 83 receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp(); //Save the second timestamp on the receiving node/anchor (T_sr)
jdawkins 0:bd42ff91c25b 84 break;
jdawkins 0:bd42ff91c25b 85 case BEACON_RESPONSE:
jdawkins 0:bd42ff91c25b 86 senderTimestamps[rangingFrame.destination][2] = dw.getTXTimestamp(); //Save the third timestamp on the sending node/beacon (T_sr)
jdawkins 0:bd42ff91c25b 87 correctSenderTimestamps(rangingFrame.destination); //Correct the timestamps for the case of a counter overflow
jdawkins 0:bd42ff91c25b 88 break;
jdawkins 0:bd42ff91c25b 89 default:
jdawkins 0:bd42ff91c25b 90 break;
jdawkins 0:bd42ff91c25b 91 }
jdawkins 0:bd42ff91c25b 92
jdawkins 0:bd42ff91c25b 93 }
jdawkins 0:bd42ff91c25b 94
jdawkins 0:bd42ff91c25b 95 /**
jdawkins 0:bd42ff91c25b 96 * Get the distance to the Anchor with address @param destination.
jdawkins 0:bd42ff91c25b 97 *
jdawkins 0:bd42ff91c25b 98 * @param destination The address of the anchor
jdawkins 0:bd42ff91c25b 99 */
jdawkins 0:bd42ff91c25b 100 void DecaWaveNetwork::requestRanging(uint8_t destination)
jdawkins 0:bd42ff91c25b 101 {
jdawkins 0:bd42ff91c25b 102 acknowledgement[destination] = false;
jdawkins 0:bd42ff91c25b 103 float time_before = LocalTimer.read();
jdawkins 0:bd42ff91c25b 104
jdawkins 0:bd42ff91c25b 105 sendPingFrame(destination);
jdawkins 0:bd42ff91c25b 106
jdawkins 0:bd42ff91c25b 107 while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.02f)); // wait for succeeding ranging or timeout
jdawkins 0:bd42ff91c25b 108
jdawkins 0:bd42ff91c25b 109 roundtriptimes[destination] = LocalTimer.read() - time_before;
jdawkins 0:bd42ff91c25b 110
jdawkins 0:bd42ff91c25b 111 if(acknowledgement[destination]) {
jdawkins 0:bd42ff91c25b 112 distances[destination] = calibratedDistance(destination);
jdawkins 0:bd42ff91c25b 113 } else {
jdawkins 0:bd42ff91c25b 114 distances[destination] = -1;
jdawkins 0:bd42ff91c25b 115 }
jdawkins 0:bd42ff91c25b 116 }
jdawkins 0:bd42ff91c25b 117
jdawkins 0:bd42ff91c25b 118 inline float DecaWaveNetwork::calibratedDistance(uint8_t destination)
jdawkins 0:bd42ff91c25b 119 {
jdawkins 0:bd42ff91c25b 120
jdawkins 0:bd42ff91c25b 121 float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
jdawkins 0:bd42ff91c25b 122
jdawkins 0:bd42ff91c25b 123 // Least Squares calibration parameters determined from dynamic data on quadrotor with Optitrack
jdawkins 0:bd42ff91c25b 124 float calibDistance = 0.9710*rawDistance - 0.5075;
jdawkins 0:bd42ff91c25b 125
jdawkins 0:bd42ff91c25b 126 // Calibration for Nucleo 0 (and 1)
jdawkins 0:bd42ff91c25b 127
jdawkins 0:bd42ff91c25b 128 // if (this->address == 1) rawDistance+= 10;
jdawkins 0:bd42ff91c25b 129 // switch(destination){
jdawkins 0:bd42ff91c25b 130 // case 2:
jdawkins 0:bd42ff91c25b 131 // return rawDistance * 0.9754 - 0.5004;
jdawkins 0:bd42ff91c25b 132 // case 3:
jdawkins 0:bd42ff91c25b 133 // return rawDistance * 0.9759 - 0.4103;
jdawkins 0:bd42ff91c25b 134 // case 4:
jdawkins 0:bd42ff91c25b 135 // return rawDistance * 0.9798 - 0.5499;
jdawkins 0:bd42ff91c25b 136 // case 5:
jdawkins 0:bd42ff91c25b 137 // return rawDistance * 0.9765 - 0.5169;
jdawkins 0:bd42ff91c25b 138 // }
jdawkins 0:bd42ff91c25b 139
jdawkins 0:bd42ff91c25b 140 return calibDistance;
jdawkins 0:bd42ff91c25b 141
jdawkins 0:bd42ff91c25b 142 }
jdawkins 0:bd42ff91c25b 143
jdawkins 0:bd42ff91c25b 144 void DecaWaveNetwork::requestRangingAll()
jdawkins 0:bd42ff91c25b 145 {
jdawkins 0:bd42ff91c25b 146 for (int i = 1; i <= 4; i++) { // Request ranging to all anchors
jdawkins 0:bd42ff91c25b 147 requestRanging(i);
jdawkins 0:bd42ff91c25b 148 }
jdawkins 0:bd42ff91c25b 149 }
jdawkins 0:bd42ff91c25b 150
jdawkins 0:bd42ff91c25b 151 void DecaWaveNetwork::sendPingFrame(uint8_t destination)
jdawkins 0:bd42ff91c25b 152 {
jdawkins 0:bd42ff91c25b 153 rangingFrame.source = address;
jdawkins 0:bd42ff91c25b 154 rangingFrame.destination = destination;
jdawkins 0:bd42ff91c25b 155 rangingFrame.type = PING;
jdawkins 0:bd42ff91c25b 156 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
jdawkins 0:bd42ff91c25b 157 }
jdawkins 0:bd42ff91c25b 158
jdawkins 0:bd42ff91c25b 159 void DecaWaveNetwork::sendTransferFrame(uint8_t destination, int timeDiffsReceiver)
jdawkins 0:bd42ff91c25b 160 {
jdawkins 0:bd42ff91c25b 161 transferFrame.source = address;
jdawkins 0:bd42ff91c25b 162 transferFrame.destination = destination;
jdawkins 0:bd42ff91c25b 163 transferFrame.type = TRANSFER_FRAME;
jdawkins 0:bd42ff91c25b 164 transferFrame.signedTime = timeDiffsReceiver; //cast the time difference
jdawkins 0:bd42ff91c25b 165 dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
jdawkins 0:bd42ff91c25b 166 }
jdawkins 0:bd42ff91c25b 167
jdawkins 0:bd42ff91c25b 168 void DecaWaveNetwork::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp)
jdawkins 0:bd42ff91c25b 169 {
jdawkins 0:bd42ff91c25b 170
jdawkins 0:bd42ff91c25b 171 rangingFrame.source = address;
jdawkins 0:bd42ff91c25b 172 rangingFrame.destination = destination;
jdawkins 0:bd42ff91c25b 173 rangingFrame.type = type;
jdawkins 0:bd42ff91c25b 174
jdawkins 0:bd42ff91c25b 175 if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
jdawkins 0:bd42ff91c25b 176 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
jdawkins 0:bd42ff91c25b 177 else
jdawkins 0:bd42ff91c25b 178 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
jdawkins 0:bd42ff91c25b 179 }
jdawkins 0:bd42ff91c25b 180
jdawkins 0:bd42ff91c25b 181 void DecaWaveNetwork::correctReceiverTimestamps(uint8_t source)
jdawkins 0:bd42ff91c25b 182 {
jdawkins 0:bd42ff91c25b 183
jdawkins 0:bd42ff91c25b 184 if(receiverTimestamps[source][0] > receiverTimestamps[source][1]) {
jdawkins 0:bd42ff91c25b 185 receiverTimestamps[source][1] += MMRANGING_2POWER40;
jdawkins 0:bd42ff91c25b 186 receiverTimestamps[source][2] += MMRANGING_2POWER40;
jdawkins 0:bd42ff91c25b 187 }
jdawkins 0:bd42ff91c25b 188
jdawkins 0:bd42ff91c25b 189 if(receiverTimestamps[source][1] > receiverTimestamps[source][2]) {
jdawkins 0:bd42ff91c25b 190 receiverTimestamps[source][2] += MMRANGING_2POWER40;
jdawkins 0:bd42ff91c25b 191 }
jdawkins 0:bd42ff91c25b 192
jdawkins 0:bd42ff91c25b 193 }
jdawkins 0:bd42ff91c25b 194
jdawkins 0:bd42ff91c25b 195 void DecaWaveNetwork::correctSenderTimestamps(uint8_t source)
jdawkins 0:bd42ff91c25b 196 {
jdawkins 0:bd42ff91c25b 197
jdawkins 0:bd42ff91c25b 198 if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
jdawkins 0:bd42ff91c25b 199 senderTimestamps[source][1] += MMRANGING_2POWER40;
jdawkins 0:bd42ff91c25b 200 senderTimestamps[source][2] += MMRANGING_2POWER40;
jdawkins 0:bd42ff91c25b 201 overflow = true;
jdawkins 0:bd42ff91c25b 202 } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
jdawkins 0:bd42ff91c25b 203 senderTimestamps[source][2] += MMRANGING_2POWER40;
jdawkins 0:bd42ff91c25b 204 overflow = true;
jdawkins 0:bd42ff91c25b 205 } else overflow = false;
jdawkins 0:bd42ff91c25b 206
jdawkins 0:bd42ff91c25b 207 }
jdawkins 0:bd42ff91c25b 208 void DecaWaveNetwork::sendMessage(uint8_t *msg,uint8_t length)
jdawkins 0:bd42ff91c25b 209 {
jdawkins 0:bd42ff91c25b 210 dw.sendFrame(msg,length);
jdawkins 0:bd42ff91c25b 211 }
jdawkins 0:bd42ff91c25b 212
jdawkins 0:bd42ff91c25b 213 void DecaWaveNetwork::parseMavlinkMsg(uint8_t msg_len)
jdawkins 0:bd42ff91c25b 214 {
jdawkins 0:bd42ff91c25b 215
jdawkins 0:bd42ff91c25b 216 //char buf[1024];
jdawkins 0:bd42ff91c25b 217 uint8_t byte;
jdawkins 0:bd42ff91c25b 218 mavlink_message_t msg;
jdawkins 0:bd42ff91c25b 219 mavlink_status_t status;
jdawkins 0:bd42ff91c25b 220
jdawkins 0:bd42ff91c25b 221
jdawkins 0:bd42ff91c25b 222 for(int i=0; i<msg_len; i++) {
jdawkins 0:bd42ff91c25b 223 memcpy(&byte,&mavlink_buffer[i],1);
jdawkins 0:bd42ff91c25b 224
jdawkins 0:bd42ff91c25b 225 if(mavlink_parse_char(MAVLINK_COMM_0,byte,&msg,&status)) {
jdawkins 0:bd42ff91c25b 226
jdawkins 0:bd42ff91c25b 227 // printf("Parse Successfully msg id %d\r\n",msg.msgid);
jdawkins 0:bd42ff91c25b 228 if(msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
jdawkins 0:bd42ff91c25b 229 mavlink_heartbeat_t hb_msg;
jdawkins 0:bd42ff91c25b 230 mavlink_msg_heartbeat_decode(&msg,&hb_msg);
jdawkins 0:bd42ff91c25b 231 //printf("System ID %d Comp ID %d \r\n",msg.sysid,msg.compid);
jdawkins 0:bd42ff91c25b 232 nodes_in_range[msg.sysid]= msg.compid;
jdawkins 0:bd42ff91c25b 233 last_heartbeat[msg.sysid]=LocalTimer.read();
jdawkins 0:bd42ff91c25b 234 // dwm_LED = !dwm_LED;
jdawkins 0:bd42ff91c25b 235 }
jdawkins 0:bd42ff91c25b 236 /* if(msg.msgid==MAVLINK_MSG_ID_FUSED_IMU) {
jdawkins 0:bd42ff91c25b 237
jdawkins 0:bd42ff91c25b 238 mavlink_fused_imu_t imu_msg;
jdawkins 0:bd42ff91c25b 239 // sendMessage(&pc,buf,n);
jdawkins 0:bd42ff91c25b 240 mavlink_msg_fused_imu_decode(&msg,&imu_msg);
jdawkins 0:bd42ff91c25b 241 imu_time = imu_msg.time_boot_ms;
jdawkins 0:bd42ff91c25b 242 roll = imu_msg.roll;
jdawkins 0:bd42ff91c25b 243 pitch = imu_msg.pitch;
jdawkins 0:bd42ff91c25b 244 yaw = imu_msg.yaw;
jdawkins 0:bd42ff91c25b 245 gx = imu_msg.gyro_x;
jdawkins 0:bd42ff91c25b 246 gy = imu_msg.gyro_y;
jdawkins 0:bd42ff91c25b 247 gz = imu_msg.gyro_z;
jdawkins 0:bd42ff91c25b 248 ax = imu_msg.accel_x;
jdawkins 0:bd42ff91c25b 249 ay = imu_msg.accel_y;
jdawkins 0:bd42ff91c25b 250 az = imu_msg.accel_z;
jdawkins 0:bd42ff91c25b 251
jdawkins 0:bd42ff91c25b 252 imu_LED = !imu_LED;
jdawkins 0:bd42ff91c25b 253 }*/
jdawkins 0:bd42ff91c25b 254
jdawkins 0:bd42ff91c25b 255 //This is from the perspective of the anchor node which doesn't compute the range
jdawkins 0:bd42ff91c25b 256 if(msg.msgid == MAVLINK_MSG_ID_RANGE_TO_NODE) {
jdawkins 0:bd42ff91c25b 257 mavlink_range_to_node_t rng_msg;
jdawkins 0:bd42ff91c25b 258 mavlink_msg_range_to_node_decode(&msg,&rng_msg);
jdawkins 0:bd42ff91c25b 259
jdawkins 0:bd42ff91c25b 260 if(rng_msg.tgt_id==address) { // if the range message is inteded for me
jdawkins 0:bd42ff91c25b 261 // rng_time = rng_msg.time_boot_ms;
jdawkins 0:bd42ff91c25b 262 ranges[rng_msg.my_id] = rng_msg.range; // my_id is the id of the sender
jdawkins 0:bd42ff91c25b 263
jdawkins 0:bd42ff91c25b 264 }
jdawkins 0:bd42ff91c25b 265 // dwm_LED = !dwm_LED;
jdawkins 0:bd42ff91c25b 266 }
jdawkins 0:bd42ff91c25b 267
jdawkins 0:bd42ff91c25b 268 }// if Mavlink Parse Returns true
jdawkins 0:bd42ff91c25b 269
jdawkins 0:bd42ff91c25b 270 }// End For number of bytes
jdawkins 0:bd42ff91c25b 271 }
jdawkins 0:bd42ff91c25b 272
jdawkins 0:bd42ff91c25b 273 void DecaWaveNetwork::checkConnectivity(){
jdawkins 0:bd42ff91c25b 274 printf("connectivity check\r\n");
jdawkins 0:bd42ff91c25b 275 for(int i=0;i<MAX_NODES;i++){
jdawkins 0:bd42ff91c25b 276 if(LocalTimer.read()-last_heartbeat[i] > 5.0){ //if its been more than 5 seconds since we recieved a heartbeat assume node is out of range
jdawkins 0:bd42ff91c25b 277 nodes_in_range[i] = 0;
jdawkins 0:bd42ff91c25b 278 }
jdawkins 0:bd42ff91c25b 279 }
jdawkins 0:bd42ff91c25b 280 }