Find correct MAC for ESPNOW

Stop searching when usng nodeID of 255 and instead use the intended MAC recepient.
gioblu · Oct 18, 2020 · f4e492d · f4e492d
1 parent 2a3a1ca
commit f4e492d
Showing 1 changed file with 8 additions and 13 deletions.
diff --git a/src/strategies/ESPNOW/ESPNOW.h b/src/strategies/ESPNOW/ESPNOW.h
@@ -61,9 +61,9 @@ class ESPNOW {
       return _espnow_initialised;
     };
 
-    int16_t find_remote_node(uint8_t id) {
+    int16_t find_remote_node(uint8_t id, uint8_t* mac) {
       for(uint8_t i = 0; i < _remote_node_count; i++)
-        if(_remote_id[i] == id)
+        if((_remote_id[i] == id && id != 255) || memcmp(_remote_mac, mac, ESP_NOW_ETH_ALEN) == 0)
           return i;
       return -1;
     };
@@ -85,21 +85,14 @@ class ESPNOW {
         en.get_sender(sender_mac);
 
         // See if PJON id is already registered, add if not
-        int16_t pos = find_remote_node(sender_id);
+        int16_t pos = find_remote_node(sender_id, sender_mac);
         if(pos == -1) {
           ESP_LOGI("ESPNOW", "Autoregister new sender %d",sender_id);
           add_node(sender_id, sender_mac);
         }
-        else if(memcmp(_remote_mac[pos], sender_mac, ESP_NOW_ETH_ALEN) != 0) {
+        else if(memcmp(_remote_mac[pos], sender_mac, ESP_NOW_ETH_ALEN) == 0) { // == 0 is the same
           // Update mac of existing node
-          ESP_LOGI(
-            "ESPNOW",
-            "Update sender mac %d %d:%d:%d",
-            sender_id,
-            sender_mac[1],
-            sender_mac[2],
-            sender_mac[3]
-          );
+          ESP_LOGI("ESPNOW","Update sender sender_id(%d) [%02X:%02X:%02X:%02X:%02X:%02X]", sender_id, sender_mac[0],sender_mac[1],sender_mac[2],sender_mac[3],sender_mac[4],sender_mac[5]);
           memcpy(_remote_mac[pos], sender_mac, ESP_NOW_ETH_ALEN);
         }
       }
@@ -213,10 +206,12 @@ class ESPNOW {
     void send_frame(uint8_t *data, uint16_t length) {
       if(length > 0) {
         uint8_t id = data[0]; // Package always starts with a receiver id
+        PJON_Packet_Info packet_info; // use parser to get intended recipient MAC
+        PJONTools::parse_header(data, packet_info);
         if(id == 0) { // Broadcast, send to all receivers
           en.send_frame(data, length);
         } else { // To a specific receiver
-          int16_t pos = find_remote_node(id);
+          int16_t pos = find_remote_node(id, packet_info.rx.mac);
           if(pos != -1)
             en.send_frame(data, length, _remote_mac[pos]);
           else //Broadcast - any replies will get registered