Diff: radioWrapper.cpp

Revision:

4:9f635ab44d8e

Parent:

2:c3ca8b8526e0

Child:

5:b8d21be6b36c

--- a/radioWrapper.cpp	Sun Jan 20 23:28:23 2019 +0000
+++ b/radioWrapper.cpp	Mon Jan 21 18:52:22 2019 +0000
@@ -2,37 +2,59 @@
 #include "mbed.h"
 #include "dbg.h"
 
-int RadioWrapper::read(uint8_t* buffer, int len, int timeout) {
+int RadioWrapper::read(uint8_t* buffer, int len, int timeout, int *pipe) {
     Timer t;
     t.start();
     // check if buffor is large enough to conaint packet.
     if (len < packetSize()) {
         return -1;
     }
+    int ret = -1;
     while(t.read_ms() < timeout) {
-        if(radio.readable(NRF24L01P_PIPE_P1)) {
-            DBG("%d", t.read_ms());
-            int ret = radio.read(NRF24L01P_PIPE_P1, (char*) buffer, packetSize());
-            DBG("RECIEVED %d, %s \r\n", ret, buffer);
-            t.stop();
-            return ret;
+        for(int i = 0; i< 3; ++i) {
+            ret = readFromPipe(buffer, len, i);
+            if(ret >0) {
+                *pipe = i;
+                return ret;
+            }
         }
     }
     t.stop();
     return 0;
 }
 
-int RadioWrapper::write(uint8_t* buffer, int len) {
-    // check if buffor is small enough to send in one package
+int RadioWrapper::readFromPipe(uint8_t* buffer, int len, int pipe) {
+     if(radio.readable(pipe)) {
+         uint8_t message[packetSize()];
+         int ret = radio.read(pipe, (char*) message, packetSize());
+         DBG("RECIEVED %d, %s \r\n", ret, buffer);
+         if(ret>0) {
+             uint8_t encodedLen = message[0];
+             std::memcpy(buffer, message+1, encodedLen);
+             return encodedLen;
+         } else {
+             return ret;
+         }
+     }
+    return 0;
+}
+
+
+int RadioWrapper::write(uint8_t* buffer, uint8_t len, int pipe) {
+    // check if buffor is small enough to send in one package + len
     if (len > packetSize()) {
         return -1;
     }
-    int ret = radio.write(NRF24L01P_PIPE_P0, (char*) buffer, packetSize());
+
+    uint8_t message[packetSize()];
+    message[0] = len;
+    std::memcpy(message+1, buffer, len);
+    int ret = radio.write(pipe, (char*) message, packetSize());
     return ret;
 }
 
-RadioWrapper::RadioWrapper(int channel, unsigned long long rx_address, unsigned long long tx_address) :
-        radio(PB_15, PB_14, PB_13, PB_12, PB_1, PB_2) {
+RadioWrapper::RadioWrapper(int channel, unsigned long long tx_address, unsigned long long rx_addresses[]) :
+    radio(PB_15, PB_14, PB_13, PB_12, PB_1, PB_2) {
     radio.powerDown();
     radio.powerUp();
 
@@ -44,10 +66,16 @@
 
     radio.setTxAddress(tx_address, 4);
     radio.setRxAddress(tx_address, 4, NRF24L01P_PIPE_P0);
-    radio.setRxAddress(rx_address, 4, NRF24L01P_PIPE_P1);
+    radio.setRxAddress(rx_addresses[0], 4, NRF24L01P_PIPE_P1);
+    radio.setRxAddress(rx_addresses[1], 4, NRF24L01P_PIPE_P2);
+    radio.setRxAddress(rx_addresses[2], 4, NRF24L01P_PIPE_P3);
+
     radio.setTransferSize(packetSize(), NRF24L01P_PIPE_P0);
     radio.setTransferSize(packetSize(), NRF24L01P_PIPE_P1);
-    radio.enableDynamicPayload(NRF24L01P_PIPE_P1);
+    radio.setTransferSize(packetSize(), NRF24L01P_PIPE_P2);
+    radio.setTransferSize(packetSize(), NRF24L01P_PIPE_P3);
+
+
     radio.setReceiveMode();
     radio.enable();