added WiFi beacon implementation

RemoteIDModule/RemoteIDModule.ino

@@ -16,6 +16,8 @@
 #include "DroneCAN.h"
 #include "WiFi_TX.h"
 #include "BLE_TX.h"
+#include <esp_wifi.h>
+#include <WiFi.h>
 #include "parameters.h"
 #include "webinterface.h"
 #include "check_firmware.h"
@@ -32,7 +34,7 @@ static MAVLinkSerial mavlink1{Serial1, MAVLINK_COMM_0};
 static MAVLinkSerial mavlink2{Serial,  MAVLINK_COMM_1};
 #endif
 
-static WiFi_NAN wifi;
+static WiFi_TX wifi;
 static BLE_TX ble;
 
 #define DEBUG_BAUDRATE 57600
@@ -61,6 +63,8 @@ void setup()
     led.set_state(Led::LedState::INIT);
     led.update();
 
+    wifi.init(); //always call the init function even if WiFi transmission modes are disabled.
+
     // Serial for debug printf
     Serial.begin(g.baudrate);
 
@@ -327,11 +331,18 @@ void loop()
     
     set_data(transport);
 
-    static uint32_t last_update_wifi_ms;
+    static uint32_t last_update_wifi_nan_ms;
     if (g.wifi_nan_rate > 0 &&
-        now_ms - last_update_wifi_ms > 1000/g.wifi_nan_rate) {
-        last_update_wifi_ms = now_ms;
-        wifi.transmit(UAS_data);
+        now_ms - last_update_wifi_nan_ms > 1000/g.wifi_nan_rate) {
+        last_update_wifi_nan_ms = now_ms;
+        wifi.transmit_nan(UAS_data);
+    }
+
+    static uint32_t last_update_wifi_beacon_ms;
+    if (g.wifi_beacon_rate > 0 &&
+        now_ms - last_update_wifi_beacon_ms > 1000/g.wifi_beacon_rate) {
+        last_update_wifi_beacon_ms = now_ms;
+        wifi.transmit_beacon(UAS_data);
     }
 
     static uint32_t last_update_bt5_ms;

RemoteIDModule/WiFi_TX.cpp

@@ -1,5 +1,5 @@
 /*
-  WiFi NAN driver
+  WiFi NAN and Beacon driver
 
   with thanks to https://github.com/sxjack/uav_electronic_ids for WiFi calls
  */
@@ -10,7 +10,7 @@
 #include <esp_system.h>
 #include "parameters.h"
 
-bool WiFi_NAN::init(void)
+bool WiFi_TX::init(void)
 {
     //use a local MAC address to avoid tracking transponders based on their MAC address
     uint8_t mac_addr[6];
@@ -22,30 +22,25 @@ bool WiFi_NAN::init(void)
     //set MAC address
     esp_base_mac_addr_set(mac_addr);
 
-    wifi_config_t wifi_config {};
+    if (g.webserver_enable == 0) {
+		WiFi.softAP(g.wifi_ssid, g.wifi_password, g.wifi_channel, false, 0); //make it visible and allow no connection
+	}
+	else {
+		WiFi.softAP(g.wifi_ssid, g.wifi_password, g.wifi_channel, false, 1); //make it visible and allow only 1 connection
+	}
 
-    WiFi.softAP("");
-
-    esp_wifi_get_config(WIFI_IF_AP, &wifi_config);
-
-    wifi_config.ap.ssid_hidden = 1;
-    wifi_config.ap.channel = wifi_channel;
-
-    if (esp_wifi_set_config(WIFI_IF_AP, &wifi_config) != ESP_OK) {
-        return false;
-    }
     if (esp_wifi_set_bandwidth(WIFI_IF_AP, WIFI_BW_HT20) != ESP_OK) {
         return false;
     }
-    if (esp_read_mac(WiFi_mac_addr, ESP_MAC_WIFI_STA) != ESP_OK) {
-        return false;
-    }
+
+    memcpy(WiFi_mac_addr,mac_addr,6); //use generated random MAC address for OpenDroneID messages
+
     esp_wifi_set_max_tx_power(dBm_to_tx_power(g.wifi_power));
 
     return true;
 }
 
-bool WiFi_NAN::transmit(ODID_UAS_Data &UAS_data)
+bool WiFi_TX::transmit_nan(ODID_UAS_Data &UAS_data)
 {
     if (!initialised) {
         initialised = true;
@@ -62,7 +57,7 @@ bool WiFi_NAN::transmit(ODID_UAS_Data &UAS_data)
     }
 
     if ((length = odid_wifi_build_message_pack_nan_action_frame(&UAS_data,(char *)WiFi_mac_addr,
-                  ++send_counter,
+                  ++send_counter_nan,
                   buffer,sizeof(buffer))) > 0) {
         if (esp_wifi_80211_tx(WIFI_IF_AP,buffer,length,true) != ESP_OK) {
             return false;
@@ -72,10 +67,62 @@ bool WiFi_NAN::transmit(ODID_UAS_Data &UAS_data)
     return true;
 }
 
+//update the payload of the beacon frames in this function
+bool WiFi_TX::transmit_beacon(ODID_UAS_Data &UAS_data)
+{
+    if (!initialised) {
+        initialised = true;
+        init();
+    }
+    uint8_t buffer[1024] {};
+
+    int length;
+    if ((length = odid_wifi_build_message_pack_beacon_frame(&UAS_data,(char *)WiFi_mac_addr,
+                   "UAS_ID_OPEN", strlen("UAS_ID_OPEN"), //use dummy SSID, as we only extract payload data
+                  1000/g.wifi_beacon_rate, ++send_counter_beacon, buffer, sizeof(buffer))) > 0) {
+
+        //set the RID IE element
+        uint8_t header_offset = 58;
+        vendor_ie_data_t IE_data;
+        IE_data.element_id = WIFI_VENDOR_IE_ELEMENT_ID;
+        IE_data.vendor_oui[0] = 0xFA;
+        IE_data.vendor_oui[1] = 0x0B;
+        IE_data.vendor_oui[2] = 0xBC;
+        IE_data.vendor_oui_type = 0x0D;
+        IE_data.length = length - header_offset + 4; //add 4 as of definition esp_wifi_set_vendor_ie
+        memcpy(IE_data.payload,&buffer[header_offset],length - header_offset);
+
+        // so first remove old element, add new afterwards
+        if (esp_wifi_set_vendor_ie(false, WIFI_VND_IE_TYPE_BEACON, WIFI_VND_IE_ID_0, &IE_data) != ESP_OK){
+            return false;
+        }
+
+        if (esp_wifi_set_vendor_ie(true, WIFI_VND_IE_TYPE_BEACON, WIFI_VND_IE_ID_0, &IE_data) != ESP_OK){
+            return false;
+        }
+
+        //set the payload also to probe requests, to increase update rate on mobile phones
+        // so first remove old element, add new afterwards
+        if (esp_wifi_set_vendor_ie(false, WIFI_VND_IE_TYPE_PROBE_RESP, WIFI_VND_IE_ID_0, &IE_data) != ESP_OK){
+            return false;
+        }
+
+        if (esp_wifi_set_vendor_ie(true, WIFI_VND_IE_TYPE_PROBE_RESP, WIFI_VND_IE_ID_0, &IE_data) != ESP_OK){
+            return false;
+        }
+
+        return true;
+	}
+	else {
+		return false;
+	}
+}
+
+
 /*
   map dBm to a TX power
  */
-uint8_t WiFi_NAN::dBm_to_tx_power(float dBm) const
+uint8_t WiFi_TX::dBm_to_tx_power(float dBm) const
 {
     if (dBm < 2) {
         dBm = 2;

RemoteIDModule/WiFi_TX.h

@@ -5,18 +5,18 @@
 
 #include "transmitter.h"
 
-class WiFi_NAN : public Transmitter {
+class WiFi_TX : public Transmitter {
 public:
     bool init(void) override;
-    bool transmit(ODID_UAS_Data &UAS_data);
+    bool transmit_nan(ODID_UAS_Data &UAS_data);
+    bool transmit_beacon(ODID_UAS_Data &UAS_data);
 
 private:
     bool initialised;
     char ssid[32];
     uint8_t WiFi_mac_addr[6];
-    uint8_t wifi_channel = 6;
     size_t ssid_length;
-    uint8_t send_counter;
-
+    uint8_t send_counter_nan;
+    uint8_t send_counter_beacon;
     uint8_t dBm_to_tx_power(float dBm) const;
 };

RemoteIDModule/parameters.cpp

@@ -17,6 +17,7 @@ const Parameters::Param Parameters::params[] = {
     { "UAS_ID",            Parameters::ParamType::CHAR20, (const void*)&g.uas_id[0],        0, 0, 0 },
     { "BAUDRATE",          Parameters::ParamType::UINT32, (const void*)&g.baudrate,         57600, 9600, 921600 },
     { "WIFI_NAN_RATE",     Parameters::ParamType::FLOAT,  (const void*)&g.wifi_nan_rate,    0, 0, 5 },
+    { "WIFI_BEACON_RATE",  Parameters::ParamType::FLOAT,  (const void*)&g.wifi_beacon_rate,    0, 0, 5 },
     { "WIFI_POWER",        Parameters::ParamType::FLOAT,  (const void*)&g.wifi_power,       20, 2, 20 },
     { "BT4_RATE",          Parameters::ParamType::FLOAT,  (const void*)&g.bt4_rate,         1, 0, 5 },
     { "BT4_POWER",         Parameters::ParamType::FLOAT,  (const void*)&g.bt4_power,        18, -27, 18 },
@@ -25,6 +26,7 @@ const Parameters::Param Parameters::params[] = {
     { "WEBSERVER_ENABLE",  Parameters::ParamType::UINT8,  (const void*)&g.webserver_enable, 1, 0, 1 },
     { "WIFI_SSID",         Parameters::ParamType::CHAR20, (const void*)&g.wifi_ssid, },
     { "WIFI_PASSWORD",     Parameters::ParamType::CHAR20, (const void*)&g.wifi_password,    0, 0, 0, PARAM_FLAG_PASSWORD, 8 },
+    { "WIFI_CHANNEL",      Parameters::ParamType::UINT8,  (const void*)&g.wifi_channel,    6, 1, 13 },
     { "BCAST_POWERUP",     Parameters::ParamType::UINT8,  (const void*)&g.bcast_powerup,    1, 0, 1 },
     { "PUBLIC_KEY1",       Parameters::ParamType::CHAR64, (const void*)&g.public_keys[0], },
     { "PUBLIC_KEY2",       Parameters::ParamType::CHAR64, (const void*)&g.public_keys[1], },

RemoteIDModule/parameters.h

@@ -20,6 +20,7 @@ public:
     uint8_t id_type;
     char uas_id[21] = "ABCD123456789";
     float wifi_nan_rate;
+    float wifi_beacon_rate;
     float wifi_power;
     float bt4_rate;
     float bt4_power;
@@ -30,6 +31,7 @@ public:
     uint8_t mavlink_sysid;
     char wifi_ssid[21] = "";
     char wifi_password[21] = "ArduRemoteID";
+    uint8_t wifi_channel = 6;
     struct {
         char b64_key[64];
     } public_keys[MAX_PUBLIC_KEYS];

RemoteIDModule/webinterface.cpp

@@ -90,7 +90,6 @@ class AJAX_Handler : public RequestHandler
 void WebInterface::init(void)
 {
     Serial.printf("WAP start %s %s\n", g.wifi_ssid, g.wifi_password);
-    WiFi.softAP(g.wifi_ssid, g.wifi_password);
     IPAddress myIP = WiFi.softAPIP();
 
     server.addHandler( &AJAX_Handler );