From b415f1d75e167d6941f1b97aa1f6ea99bf1bada3 Mon Sep 17 00:00:00 2001 From: Mascaro Lucas Date: Thu, 13 Dec 2018 15:39:03 +0100 Subject: [PATCH] Implement sendToNearest --- node/main/main.ino | 14 +++---- well/main/main.ino | 9 +++-- xbee_wrapper.cpp | 94 +++++++++++++++++++++++++++++++++++----------- xbee_wrapper.h | 15 +++++++- 4 files changed, 98 insertions(+), 34 deletions(-) diff --git a/node/main/main.ino b/node/main/main.ino index a5698f7..4b34205 100644 --- a/node/main/main.ino +++ b/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); } diff --git a/well/main/main.ino b/well/main/main.ino index 65cea59..0b8c752 100644 --- a/well/main/main.ino +++ b/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(); diff --git a/xbee_wrapper.cpp b/xbee_wrapper.cpp index fa76377..002b82e 100644 --- a/xbee_wrapper.cpp +++ b/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; +}; diff --git a/xbee_wrapper.h b/xbee_wrapper.h index 0bd18f1..6fc43b4 100644 --- a/xbee_wrapper.h +++ b/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,8 +39,11 @@ // 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); + }; -#endif \ No newline at end of file +#endif