Diff: decawave_network.cpp

Revision:

4:01caa2302657

Parent:

1:93fcc351837a

diff -r 93fcc351837a -r 01caa2302657 decawave_network.cpp
--- a/decawave_network.cpp	Wed Feb 15 17:51:48 2017 +0000
+++ b/decawave_network.cpp	Thu Feb 16 17:40:00 2017 +0000
@@ -14,7 +14,7 @@
     dw.setCallbacks(this, &DecaWaveNetwork::callbackRX, &DecaWaveNetwork::callbackTX);
 
     LocalTimer.start();
-    checkConn.attach(this,&DecaWaveNetwork::checkConnectivity, 10.0);
+    //checkConn.attach(this,&DecaWaveNetwork::checkConnectivity, 10.0);
     dw.startRX();
 }
 
@@ -228,7 +228,7 @@
             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);
+              //  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;
@@ -256,10 +256,11 @@
             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;
+                    printf("Range from %d to %d is %f\r\n", rng_msg.my_id,rng_msg.tgt_id,rng_msg.range);
                     ranges[rng_msg.my_id] = rng_msg.range; // my_id is the id of the sender
+                    distances[rng_msg.my_id] = rng_msg.range;
                     
                 }
                 //      dwm_LED = !dwm_LED;