Diff: decawave_network.cpp

Revision:

0:bd42ff91c25b

Child:

1:93fcc351837a

diff -r 000000000000 -r bd42ff91c25b decawave_network.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/decawave_network.cpp	Wed Feb 15 17:23:19 2017 +0000
@@ -0,0 +1,280 @@
+#include "decawave_network.h"
+
+DecaWaveNetwork::DecaWaveNetwork(DW1000& DW) : dw(DW)
+{
+    isAnchor = true;
+    overflow = false;
+    address = 0;
+    rxTimestamp = 0;
+    timediffRec = 0;
+    timediffSend = 0;
+    for (int i = 0; i < MAX_NODES; i++)
+        acknowledgement[i] = true;
+
+    dw.setCallbacks(this, &DecaWaveNetwork::callbackRX, &DecaWaveNetwork::callbackTX);
+
+    LocalTimer.start();
+    checkConn.attach(this,&DecaWaveNetwork::checkConnectivity, 10.0);
+    dw.startRX();
+}
+
+void DecaWaveNetwork::callbackRX()
+{
+    int n = dw.getFramelength();
+    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, n);
+
+
+
+    if (receivedFrame.destination == address){
+        switch (receivedFrame.type) {
+            case PING:
+                rxTimestamp = dw.getRXTimestamp();
+                receiverTimestamps[receivedFrame.source][0] = rxTimestamp;      //Save the first timestamp on the receiving node/anchor (T_rp)
+                sendDelayedAnswer(receivedFrame.source, ANCHOR_RESPONSE, rxTimestamp);
+                break;
+            case ANCHOR_RESPONSE:
+                rxTimestamp = dw.getRXTimestamp();
+                senderTimestamps[receivedFrame.source][1] = rxTimestamp;        //Save the second timestamp on the sending node/beacon (T_rr)
+                sendDelayedAnswer(receivedFrame.source, 3, rxTimestamp);
+                break;
+            case BEACON_RESPONSE:
+                rxTimestamp = dw.getRXTimestamp();
+                receiverTimestamps[receivedFrame.source][2] = rxTimestamp;      //Save the third timestamp on the receiving node/anchor (T_rf)
+
+                correctReceiverTimestamps(receivedFrame.source);                //Correct the timestamps for the case of a counter overflow
+                //calculation of the summand on the receiving node/anchor
+                timediffRec = - 2*receiverTimestamps[receivedFrame.source][1] + receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2];
+                sendTransferFrame(receivedFrame.source, timediffRec );
+                break;
+            case TRANSFER_FRAME:
+                //calculation of the summand on the sending node/beacon
+                timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
+                //calculation of the resulting sum of all four ToFs.
+                tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
+                acknowledgement[receivedFrame.source] = true;
+                break;
+            default :
+                break;
+        }
+    } else{
+        
+    memcpy(&mavlink_buffer,&receivedFrame,n);
+   // int i;
+
+   /* for(int i=0; i<n; i++) {
+        printf("%X",mavlink_buffer[i]);
+    }
+    printf("\r\n");*/
+    } // end else
+
+
+    parseMavlinkMsg(n);
+
+    dw.startRX();
+}
+
+void DecaWaveNetwork::callbackTX()
+{
+    switch (rangingFrame.type) {
+        case PING:
+            senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp();    //Save the first timestamp on the sending node/beacon (T_sp)
+            break;
+        case ANCHOR_RESPONSE:
+            receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp();  //Save the second timestamp on the receiving node/anchor (T_sr)
+            break;
+        case BEACON_RESPONSE:
+            senderTimestamps[rangingFrame.destination][2] = dw.getTXTimestamp();    //Save the third timestamp on the sending node/beacon (T_sr)
+            correctSenderTimestamps(rangingFrame.destination);                      //Correct the timestamps for the case of a counter overflow
+            break;
+        default:
+            break;
+    }
+
+}
+
+/**
+ *  Get the distance to the Anchor with address @param destination.
+ *
+ *   @param destination The address of the anchor
+ */
+void DecaWaveNetwork::requestRanging(uint8_t destination)
+{
+    acknowledgement[destination] = false;
+    float time_before = LocalTimer.read();
+
+    sendPingFrame(destination);
+
+    while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.02f)); // wait for succeeding ranging or timeout
+
+    roundtriptimes[destination] = LocalTimer.read() - time_before;
+
+    if(acknowledgement[destination]) {
+        distances[destination] = calibratedDistance(destination);
+    } else {
+        distances[destination] = -1;
+    }
+}
+
+inline float DecaWaveNetwork::calibratedDistance(uint8_t destination)
+{
+
+    float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
+
+// Least Squares calibration parameters determined from dynamic data on quadrotor with Optitrack
+    float calibDistance = 0.9710*rawDistance - 0.5075;
+
+// Calibration for Nucleo 0 (and 1)
+
+//   if (this->address == 1) rawDistance+= 10;
+//    switch(destination){
+//        case 2:
+//            return  rawDistance * 0.9754 - 0.5004;
+//        case 3:
+//            return  rawDistance * 0.9759 - 0.4103;
+//        case 4:
+//            return  rawDistance * 0.9798 - 0.5499;
+//        case 5:
+//            return  rawDistance * 0.9765 - 0.5169;
+//        }
+
+    return calibDistance;
+
+}
+
+void DecaWaveNetwork::requestRangingAll()
+{
+    for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
+        requestRanging(i);
+    }
+}
+
+void DecaWaveNetwork::sendPingFrame(uint8_t destination)
+{
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = PING;
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+}
+
+void DecaWaveNetwork::sendTransferFrame(uint8_t destination, int timeDiffsReceiver)
+{
+    transferFrame.source = address;
+    transferFrame.destination = destination;
+    transferFrame.type = TRANSFER_FRAME;
+    transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
+    dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
+}
+
+void DecaWaveNetwork::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp)
+{
+
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = type;
+
+    if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
+        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
+    else
+        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
+}
+
+void DecaWaveNetwork::correctReceiverTimestamps(uint8_t source)
+{
+
+    if(receiverTimestamps[source][0] > receiverTimestamps[source][1]) {
+        receiverTimestamps[source][1] += MMRANGING_2POWER40;
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+    if(receiverTimestamps[source][1] > receiverTimestamps[source][2]) {
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+}
+
+void DecaWaveNetwork::correctSenderTimestamps(uint8_t source)
+{
+
+    if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
+        senderTimestamps[source][1] += MMRANGING_2POWER40;
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+        overflow = true;
+    } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+        overflow = true;
+    } else overflow = false;
+
+}
+void DecaWaveNetwork::sendMessage(uint8_t *msg,uint8_t length)
+{
+    dw.sendFrame(msg,length);
+}
+
+void DecaWaveNetwork::parseMavlinkMsg(uint8_t msg_len)
+{
+
+    //char buf[1024];
+    uint8_t byte;
+    mavlink_message_t msg;
+    mavlink_status_t status;
+
+
+    for(int i=0; i<msg_len; i++) {
+        memcpy(&byte,&mavlink_buffer[i],1);
+
+        if(mavlink_parse_char(MAVLINK_COMM_0,byte,&msg,&status)) {
+
+           // printf("Parse Successfully msg id %d\r\n",msg.msgid);
+            if(msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
+                mavlink_heartbeat_t hb_msg;
+                mavlink_msg_heartbeat_decode(&msg,&hb_msg);
+                //printf("System ID %d Comp ID %d \r\n",msg.sysid,msg.compid);
+                nodes_in_range[msg.sysid]= msg.compid;
+                last_heartbeat[msg.sysid]=LocalTimer.read();
+                //    dwm_LED = !dwm_LED;
+            }
+            /* if(msg.msgid==MAVLINK_MSG_ID_FUSED_IMU) {
+
+                 mavlink_fused_imu_t imu_msg;
+                 // sendMessage(&pc,buf,n);
+                 mavlink_msg_fused_imu_decode(&msg,&imu_msg);
+                 imu_time = imu_msg.time_boot_ms;
+                 roll = imu_msg.roll;
+                 pitch = imu_msg.pitch;
+                 yaw = imu_msg.yaw;
+                 gx = imu_msg.gyro_x;
+                 gy = imu_msg.gyro_y;
+                 gz = imu_msg.gyro_z;
+                 ax = imu_msg.accel_x;
+                 ay = imu_msg.accel_y;
+                 az = imu_msg.accel_z;
+
+                 imu_LED = !imu_LED;
+             }*/
+
+            //This is from the perspective of the anchor node which doesn't compute the range
+            if(msg.msgid == MAVLINK_MSG_ID_RANGE_TO_NODE) {
+                mavlink_range_to_node_t rng_msg;
+                mavlink_msg_range_to_node_decode(&msg,&rng_msg);
+
+                if(rng_msg.tgt_id==address) { // if the range message is inteded for me
+                    // rng_time = rng_msg.time_boot_ms;
+                    ranges[rng_msg.my_id] = rng_msg.range; // my_id is the id of the sender
+                    
+                }
+                //      dwm_LED = !dwm_LED;
+            }
+
+        }// if Mavlink Parse Returns true
+
+    }// End For number of bytes
+}
+
+void DecaWaveNetwork::checkConnectivity(){
+    printf("connectivity check\r\n");
+    for(int i=0;i<MAX_NODES;i++){
+        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
+            nodes_in_range[i] = 0;
+        }
+    }
+}
\ No newline at end of file