From d04639a8655c571944475740683e5fd8a7737d56 Mon Sep 17 00:00:00 2001
From: Timm Bogner <64260873+timmbogner@users.noreply.github.com>
Date: Fri, 22 Jul 2022 01:37:57 -0500
Subject: [PATCH] Finished ping support

Not immediately useful, but it is interesting to know how low an FDRS ping is (in ms). I won't ruin the surprise, but it's pretty low.

More importantly however, I laid out how SystemPackets will work. I might rename those.
---
 FDRS_Gateway/fdrs_functions.h      | 17 ++++++++++-
 FDRS_Gateway/fdrs_gateway_config.h |  2 +-
 FDRS_Sensor/FDRS_Sensor.ino        |  1 +
 FDRS_Sensor/fdrs_sensor.h          | 49 ++++++++++++++++++++++++++++++
 FDRS_Sensor/fdrs_sensor_config.h   |  3 +-
 5 files changed, 69 insertions(+), 3 deletions(-)

diff --git a/FDRS_Gateway/fdrs_functions.h b/FDRS_Gateway/fdrs_functions.h
index 2e17c8f..40427fa 100644
--- a/FDRS_Gateway/fdrs_functions.h
+++ b/FDRS_Gateway/fdrs_functions.h
@@ -729,9 +729,24 @@ void handleCommands() {
   switch (theCmd.cmd) {
     case cmd_ping:
       DBG("Ping back to sender");
+      SystemPacket sys_packet;
+      sys_packet.cmd = cmd_ping;
+#if defined(ESP32)
+      esp_now_peer_info_t peerInfo;
+      peerInfo.ifidx = WIFI_IF_STA;
+      peerInfo.channel = 0;
+      peerInfo.encrypt = false;
+      memcpy(peerInfo.peer_addr, incMAC, 6);
+      if (esp_now_add_peer(&peerInfo) != ESP_OK) {
+        DBG("Failed to add peer");
+        return;
+      }
+#endif
+      esp_now_send(incMAC, (uint8_t *) &sys_packet, sizeof(SystemPacket));
+        esp_now_del_peer(incMAC);
       break;
     case cmd_add:
-      DBG("Add sender to peer list");
+      DBG("Add sender to peer list (not completed)");
       break;
   }
   theCmd.cmd = cmd_clear;
diff --git a/FDRS_Gateway/fdrs_gateway_config.h b/FDRS_Gateway/fdrs_gateway_config.h
index 49c4758..c50321e 100644
--- a/FDRS_Gateway/fdrs_gateway_config.h
+++ b/FDRS_Gateway/fdrs_gateway_config.h
@@ -5,7 +5,7 @@
 #include <fdrs_globals.h>
 #define FDRS_DEBUG     //Enable USB-Serial debugging
 
-#define UNIT_MAC     0x01  // The address of this gateway
+#define UNIT_MAC     0x04  // The address of this gateway
 
 // Actions -- Define what happens when a packet arrives at each interface:
 // Current function options are: sendESPNOW(MAC), sendSerial(), sendMQTT(),
diff --git a/FDRS_Sensor/FDRS_Sensor.ino b/FDRS_Sensor/FDRS_Sensor.ino
index f034294..0d89c1b 100644
--- a/FDRS_Sensor/FDRS_Sensor.ino
+++ b/FDRS_Sensor/FDRS_Sensor.ino
@@ -17,6 +17,7 @@ void setup() {
   beginFDRS();
 }
 void loop() {
+  pingFDRS(1000);
   data1 = readHum();
   loadFDRS(data1, HUMIDITY_T);
   data2 = readTemp();
diff --git a/FDRS_Sensor/fdrs_sensor.h b/FDRS_Sensor/fdrs_sensor.h
index 0bbf50f..829b7ed 100644
--- a/FDRS_Sensor/fdrs_sensor.h
+++ b/FDRS_Sensor/fdrs_sensor.h
@@ -59,6 +59,27 @@ uint8_t LoRaAddress[] = {0x42, 0x00};
 uint32_t wait_time = 0;
 DataReading fdrsData[espnow_size];
 uint8_t data_count = 0;
+bool is_ping = false;
+
+// Set ESP-NOW send and receive callbacks for either ESP8266 or ESP32
+#if defined(ESP8266)
+void OnDataSent(uint8_t *mac_addr, uint8_t sendStatus) {
+}
+void OnDataRecv(uint8_t* mac, uint8_t *incomingData, uint8_t len) {
+#elif defined(ESP32)
+void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
+}
+void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
+#endif
+  if (len < sizeof(DataReading)) {
+    SystemPacket command;
+    memcpy(&command, incomingData, sizeof(command));
+    if (command.cmd == cmd_ping) {
+      is_ping = true;
+      return;
+    }
+  }
+}
 
 void beginFDRS() {
 #ifdef FDRS_DEBUG
@@ -81,13 +102,18 @@ void beginFDRS() {
     return;
   }
   esp_now_set_self_role(ESP_NOW_ROLE_COMBO);
+  esp_now_register_recv_cb(OnDataRecv);
+
   // Register peers
   esp_now_add_peer(gatewayAddress, ESP_NOW_ROLE_COMBO, 0, NULL, 0);
 #elif defined(ESP32)
+
   if (esp_now_init() != ESP_OK) {
     DBG("Error initializing ESP-NOW");
     return;
   }
+  esp_now_register_recv_cb(OnDataRecv);
+
   esp_now_peer_info_t peerInfo;
   peerInfo.ifidx = WIFI_IF_STA;
   peerInfo.channel = 0;
@@ -166,3 +192,26 @@ void sleepFDRS(int sleep_time) {
   DBG(" Delaying.");
   delay(sleep_time * 1000);
 }
+
+void pingFDRS(int timeout) {
+  SystemPacket sys_packet;
+  sys_packet.cmd = cmd_ping;
+#ifdef USE_ESPNOW
+  esp_now_send(gatewayAddress, (uint8_t *) &sys_packet, sizeof(SystemPacket));
+  DBG(" ESP-NOW ping sent.");
+  uint32_t ping_start = millis();
+  is_ping = false;
+  while ((millis() - ping_start) <= timeout) {
+    yield(); //do I need to yield or does it automatically?
+    if (is_ping) {
+      DBG("Ping Returned:" + String(millis() - ping_start));
+      break;
+    }
+  }
+
+#endif
+#ifdef USE_LORA
+  //transmitLoRa(gtwyAddress, sys_packet, data_count); // TODO: Make this congruent to esp_now_send()
+  DBG(" LoRa ping not sent because it isn't implemented.");
+#endif
+}
diff --git a/FDRS_Sensor/fdrs_sensor_config.h b/FDRS_Sensor/fdrs_sensor_config.h
index 2a3fdde..06fa987 100644
--- a/FDRS_Sensor/fdrs_sensor_config.h
+++ b/FDRS_Sensor/fdrs_sensor_config.h
@@ -5,12 +5,13 @@
 #include <fdrs_globals.h>
 
 #define READING_ID    1   //Unique ID for this sensor
-#define GTWY_MAC      0x01 //Address of the nearest gateway
+#define GTWY_MAC      0x04 //Address of the nearest gateway
 
 #define USE_ESPNOW
 //#define USE_LORA
 #define DEEP_SLEEP
 //#define POWER_CTRL    14
+#define DYNAMIC_GTWY
 #define FDRS_DEBUG
 
 //SPI Configuration -- Needed only on chipsets with multiple SPI interfaces (ESP32)