Implement sendToNearest

node/main/main.ino

@@ -6,14 +6,14 @@
 
 #define SEND_TIMEOUT 10000
 
-// Peripherals
-LCDWrapper screen(0x20, 16, 2);
-XBeeWrapper xbee = XBeeWrapper();
-
 // ACTUAL DATA
 unsigned long time;
 Node myself = Node();
 
+// Peripherals
+LCDWrapper screen(0x20, 16, 2);
+XBeeWrapper xbee = XBeeWrapper(myself);
+
 void setup() {
 	Serial.begin(38400);
 	Serial.println("+ ready");
@@ -64,7 +64,7 @@ void send_data(){
 	send.setData(data);
 
 	// 2. broadcast
-	xbee.broadcast(send);
+	xbee.sendToNearest(send);
 
  // 2. debug
   screen.clear();
@@ -126,13 +126,11 @@ void manage_message(Packet recv){
 	}
 
 	// propagation
-	// send.setOpcode(OPCODE_MESSAGE);
 	recv.setTTL(recv.getTTL()-1);
 	recv.setDist(myself.getDist());
-	// send.setData(recv.getData());
 
 	screen.printfn(1, "msg[%3d/%3d] %3d", recv.getDist(), recv.getTTL(), recv.getSize());
 	delay(500);
 	screen.printfn(1, "%s", (char*) recv.getData());
-	xbee.broadcast(recv);
+	xbee.sendToNearest(recv);
 }

well/main/main.ino

@@ -3,14 +3,15 @@
 #include "xbee_wrapper.h"
 #include "packet.h"
 
+//Setup
+Node myself = Node();
+
 // Peripherals
 LCDWrapper screen(0x20, 16, 2);
-XBeeWrapper xbee = XBeeWrapper();
+XBeeWrapper xbee = XBeeWrapper(myself);
 
 // ACTUAL DATA
 uint8_t wave_id = 250;
-Packet recv = Packet();
-Packet send = Packet();
 unsigned long time;
 
 void setup() {
@@ -32,6 +33,7 @@ void loop() {
 		time = millis();
 
 		// increment wave id (will overflow from 255 to 0)
+    Packet send;
 		send.setOpcode(OPCODE_DISCOVER);
 		send.setWave(++wave_id);
 		send.setDist(0);
@@ -44,6 +46,7 @@ void loop() {
 	}
 
 	/* [2] Listen for incoming data */
+  Packet recv;
 	if( xbee.receive(recv) != XBWRECV_OK )
 		return;
 	uint8_t opcode = recv.getOpcode();

xbee_wrapper.cpp

@@ -1,7 +1,10 @@
 #include "xbee_wrapper.h"
 
 
-XBeeWrapper::XBeeWrapper(){ xbee = XBee(); };
+XBeeWrapper::XBeeWrapper(Node &_node){ 
+  xbee = XBee();
+  node = _node;
+};
 
 
 void XBeeWrapper::begin(const unsigned long baud){
@@ -11,37 +14,59 @@ void XBeeWrapper::begin(const unsigned long baud){
 
 
 uint8_t XBeeWrapper::receive(Packet& pkt){
-	xbee.readPacket();
+  xbee.readPacket();
 
-	if( !xbee.getResponse().isAvailable() )
-		return XBWRECV_NONE;
+  if( !xbee.getResponse().isAvailable() )
+    return XBWRECV_NONE;
 
-	uint8_t apiId = xbee.getResponse().getApiId();
-	if( apiId != RX_16_RESPONSE && apiId != RX_64_RESPONSE )
-		return XBWRECV_NONE;
+  uint8_t apiId = xbee.getResponse().getApiId();
 
-	if( apiId == RX_16_RESPONSE ){
+  uint8_t packetRssi = 0;
+  uint64_t packetMsb = 0;
+  uint64_t packetLsb = 0;
 
-		Rx16Response res = Rx16Response();
-		xbee.getResponse().getRx16Response(res);
+  if( apiId != RX_16_RESPONSE && apiId != RX_64_RESPONSE )
+    return XBWRECV_NONE;
 
-		// extract data and propagate error
-		if( pkt.read(res.getData(), res.getDataLength()) != PKTREAD_OK)
-			return XBWRECV_ERROR;
+  if( apiId == RX_16_RESPONSE ){
 
-	} else {
+    Rx16Response res = Rx16Response();
+    xbee.getResponse().getRx16Response(res);
 
-		Rx64Response res = Rx64Response();
-		xbee.getResponse().getRx64Response(res);
+    packetRssi = res.getRssi();
+    msb = res.getRemoteAddress16();
 
-		// extract data and propagate error
-		if( pkt.read(res.getData(), res.getDataLength()) != PKTREAD_OK)
-			return XBWRECV_ERROR;
+    // extract data and propagate error
+    if( pkt.read(res.getData(), res.getDataLength()) != PKTREAD_OK)
+      return XBWRECV_ERROR;
 
-	}
+  } else {
 
+    Rx64Response res = Rx64Response();
+    xbee.getResponse().getRx64Response(res);
 
-	return XBWRECV_OK;
+    packetRssi = res.getRssi();
+    packetMsb = res.getRemoteAddress64().getMsb();
+    packetLsb = res.getRemoteAddress64().getLsb();
+
+    // extract data and propagate error
+    if( pkt.read(res.getData(), res.getDataLength()) != PKTREAD_OK)
+      return XBWRECV_ERROR;
+
+  }
+
+  if(pkt.getOpcode() != OPCODE_DISCOVER)
+    return XBWRECV_OK;
+
+  //if the wave id changed, or if the last rssi is higher than the new one change the rssi
+  if(node.update(pkt.getDist(),pkt.getWave()) || (rssi > packetRssi && pkt.getDist() < node.getDist())){
+    rssi = packetRssi;
+    addrType = apiId;
+    msb = packetMsb;
+    lsb = packetLsb;
+  }
+
+  return XBWRECV_OK;
 };
 
 uint8_t XBeeWrapper::broadcast(Packet& pkt){
@@ -59,3 +84,30 @@ uint8_t XBeeWrapper::broadcast(Packet& pkt){
 
 	return XBWSEND_OK;
 };
+
+uint32_t XBeeWrapper::sendToNearest(Packet& pkt){
+  if(addrType != RX_64_RESPONSE && addrType != RX_16_RESPONSE )
+    return XBWSEND_NONEAREST;
+
+  // build payload from packet
+  uint8_t payload[PROTO_SIZE];
+  memset(payload,0,PROTO_SIZE);
+  size_t payload_size = pkt.write(payload);
+
+  XBeeRequest* tx;
+  if(addrType == RX_64_RESPONSE){
+    // build broadcast addr
+    XBeeAddress64 addr = XBeeAddress64(msb, lsb);
+    
+    // instanciate
+    tx = new Tx64Request(addr, payload, payload_size);   
+  }else{
+    // instanciate
+    tx = new Tx16Request((uint16_t) msb, payload, payload_size);
+  }
+
+  //send
+  xbee.send(*tx);
+
+  return XBWSEND_OK;
+};

xbee_wrapper.h

@@ -13,14 +13,22 @@
 	#define XBWSEND_OK      0
 	#define XBWSEND_ERROR   1
 	#define XBWSEND_UNKNOWN 2
+  #define XBWSEND_NONEAREST 3
 
+  #define RX_NONE_RESPONSE 0
 
 	class XBeeWrapper{
 		private:
 			XBee xbee;
+      Node node;
+
+      uint8_t rssi = 255;
+      uint8_t addrType = RX_NONE_RESPONSE;
+      uint64_t msb = 0;
+      uint64_t lsb = 0;
 
 		public:
-			XBeeWrapper();
+			XBeeWrapper(Node &node);
 
 			// initialises the XBee interface
 			void begin(const unsigned long baud);
@@ -31,6 +39,9 @@
 			// tries to broadcast a packet
 			uint8_t broadcast(Packet& pkt);
 
+      // tries to send to the nearest node (of a lower level)
+      uint32_t sendToNearest(Packet& pkt);
+
 	};