Update ESP32_BluetoothSerial.ino

Author: PaulZC

examples/ESP32_BluetoothSerial/ESP32_BluetoothSerial.ino

@@ -19,8 +19,8 @@
 
   NMEA GGA and RMC are shared with apps like Maps, over the Control Session
   as Location Information.
-  NMEA GGA, RMC and GST are shared with apps like Field Maps, over a dedicated
-  External Accessory protocol session.
+  NMEA GGA, RMC, GST, VTG, GSA and GSV are shared with apps like Field Maps,
+  over a dedicated External Accessory protocol session.
 
   This example contains a copy of Espressif's BluetoothSerial code, with a
   modified ESP_BT_GAP_ACL_CONN_CMPL_STAT_EVT event.
@@ -50,18 +50,24 @@
   (SDP is disabled in the standard libbt.a)
   It also corrects the UUID byte-order reversal in add_raw_sdp
 
-  Tried and tested on: Arduino esp32 v3.0.7 (IDF 5.1)
+  Written for and tested on: Arduino esp32 v3.0.7 (IDF 5.1)
 
 */
 
+// ===================================================================================================================
+// ESP32 Service Discovery Protocol
+
+#include "esp_sdp_api.h"
+
 // ===================================================================================================================
 // GNSS
 
 #include <SparkFun_u-blox_GNSS_v3.h> //Click here to get the library: http://librarymanager/All#SparkFun_u-blox_GNSS_v3
 SFE_UBLOX_GNSS myGNSS;
 char latestGPGGA[SFE_UBLOX_MAX_NMEA_BYTE_COUNT] = { 0 };
 char latestGPRMC[SFE_UBLOX_MAX_NMEA_BYTE_COUNT] = { 0 };
-char latestGPGST[SFE_UBLOX_MAX_NMEA_BYTE_COUNT] = { 0 };
+const size_t additionalNmeaBufferSize = 4096;
+char additionalNmeaBuffer[additionalNmeaBufferSize] = { 0 };
 
 void newGPGGA(NMEA_GGA_data_t *nmeaData)
 {
@@ -83,25 +89,15 @@ void newGPRMC(NMEA_RMC_data_t *nmeaData)
   }
 }
 
-void newGPGST(NMEA_GST_data_t *nmeaData)
-{
-  if (nmeaData->length < SFE_UBLOX_MAX_NMEA_BYTE_COUNT)
-  {
-    strncpy(latestGPGST, (const char *)nmeaData->nmea, nmeaData->length);
-    //Serial.print(latestGPGST); // .nmea is printable (NULL-terminated) and already has \r\n on the end
-    latestGPGST[nmeaData->length - 2] = 0; // Truncate after checksum. Remove CR LF
-  }
-}
-
 // ===================================================================================================================
 // Apple Accessory
 
 #include <SparkFun_Apple_Accessory.h>
 
 SparkFunAppleAccessoryDriver appleAccessory;
 
-const char *accessoryName = "SparkFun MFi Test";
-const char *modelIdentifier = "SparkFun MFi Test";
+const char *accessoryName = "SparkPNT RTK Flex";
+const char *modelIdentifier = "SparkPNT RTK Flex";
 const char *manufacturer = "SparkFun Electronics";
 const char *serialNumber = "123456";
 const char *firmwareVersion = "1.0.0";
@@ -117,10 +113,10 @@ const char *productPlanUID = "0123456789ABCDEF"; // This comes from the MFi Port
 #include "src/BluetoothSerial/BluetoothSerial.h" //Local copy for modifying ESP_BT_GAP_ACL_CONN_CMPL_STAT_EVT event
 BluetoothSerial SerialBT;
 
-#include "esp_sdp_api.h"
-
 const char *sdp_service_name = "iAP2";
 
+const int rfcommChanneliAP2 = 2; // We can use RFCOMM channel 2 for iAP2
+
 // With the byte-order correction in libbt.a btc_sdp.c add_raw_sdp() (ARRAY_TO_BE_STREAM_REVERSE replaces ARRAY_TO_BE_STREAM),
 // we need to provide the UUID as little-endian:
 // UUID : Little-Endian
@@ -138,6 +134,52 @@ void transportDisconnect(bool *disconnected)
   *disconnected = SerialBT.disconnect();
 }
 
+// ===================================================================================================================
+// Callback for Service Discovery Protocol
+// This allows the iAP2 record to be created _after_ SDP is initialized
+// and the Serial Port (SPP) record has been created
+
+volatile bool sdpCreateRecordEvent = false; // Flag to indicate when the iAP2 record has been created
+
+static void esp_sdp_callback(esp_sdp_cb_event_t event, esp_sdp_cb_param_t *param)
+{
+    switch (event) {
+    case ESP_SDP_INIT_EVT:
+        Serial.printf("ESP_SDP_INIT_EVT: status: %d\r\n", param->init.status);
+        if (param->init.status == ESP_SDP_SUCCESS) {
+            // SDP has been initialized. _Now_ we can create the iAP2 record!
+            esp_bluetooth_sdp_hdr_overlay_t record = {(esp_bluetooth_sdp_types_t)0};
+            record.type = ESP_SDP_TYPE_RAW;
+            record.uuid.len = sizeof(UUID_IAP2);
+            memcpy(record.uuid.uuid.uuid128, UUID_IAP2, sizeof(UUID_IAP2));
+            // The service_name isn't critical. But we can't not set one.
+            // (If we don't set a name, the record doesn't get created.)
+            record.service_name_length = strlen(sdp_service_name) + 1;
+            record.service_name = (char *)sdp_service_name;
+            record.rfcomm_channel_number = rfcommChanneliAP2; // RFCOMM channel
+            record.l2cap_psm = -1;
+            record.profile_version = -1;
+            esp_sdp_create_record((esp_bluetooth_sdp_record_t *)&record);
+        }
+        break;
+    case ESP_SDP_DEINIT_EVT:
+        Serial.printf("ESP_SDP_DEINIT_EVT: status: %d\r\n", param->deinit.status);
+        break;
+    case ESP_SDP_SEARCH_COMP_EVT:
+        Serial.printf("ESP_SDP_SEARCH_COMP_EVT: status: %d\r\n", param->search.status);
+        break;
+    case ESP_SDP_CREATE_RECORD_COMP_EVT:
+        Serial.printf("ESP_SDP_CREATE_RECORD_COMP_EVT: status: %d\r\n", param->create_record.status);
+        sdpCreateRecordEvent = true; // Flag that the iAP2 record has been created
+        break;
+    case ESP_SDP_REMOVE_RECORD_COMP_EVT:
+        Serial.printf("ESP_SDP_REMOVE_RECORD_COMP_EVT: status: %d\r\n", param->remove_record.status);
+        break;
+    default:
+        break;
+    }
+}
+
 // ===================================================================================================================
 
 void setup()
@@ -150,9 +192,10 @@ void setup()
   // ==============================================================================================================
   // Setup Bluetooth
 
-  SerialBT.enableSSP(false, true); //Enable secure pairing with PIN
+  SerialBT.enableSSP(false, false); //Enable secure pairing
 
-  SerialBT.begin(accessoryName, true, true);  //Bluetooth device name, start in master mode, disable BLE
+  //Bluetooth device name, master mode, disable BLE, rxQueueSize, txQueueSize
+  SerialBT.begin(accessoryName, false, true, 2048, 512);
 
   Serial.println("The BT device was started.");
 
@@ -166,16 +209,10 @@ void setup()
   // (But, deleting them is essential if you have changed the secure pairing mode)
   Serial.println("Deleting all previous bonded devices.");
   SerialBT.deleteAllBondedDevices(); // Must be called after begin
-  
-  esp_sdp_init();
 
-  esp_bluetooth_sdp_hdr_overlay_t record = {(esp_bluetooth_sdp_types_t)0};
-  record.type = ESP_SDP_TYPE_RAW;
-  record.uuid.len = sizeof(UUID_IAP2);
-  memcpy(record.uuid.uuid.uuid128, UUID_IAP2, sizeof(UUID_IAP2));
-  record.service_name_length = strlen(accessoryName) + 1;
-  record.service_name = (char *)accessoryName;
-  esp_sdp_create_record((esp_bluetooth_sdp_record_t *)&record);
+  // The SDP callback will create the iAP2 record
+  esp_sdp_register_callback(esp_sdp_callback);
+  esp_sdp_init();
 
   // ==============================================================================================================
   // Setup Apple Accessory and Authentication Coprocessor
@@ -204,8 +241,13 @@ void setup()
   appleAccessory.setLocationInfoComponentName(LIComponentName);
   appleAccessory.setProductPlanUID(productPlanUID);
 
-  // Pass the pointers for the latest NMEA data into the Accessory driver
-  appleAccessory.setNMEApointers(latestGPGGA, latestGPRMC, latestGPGST);
+  // Pass the pointers for the latest NMEA GGA and RMC data into the Accessory driver
+  // GGA and RMC are passed to apps over the iAP2 Control Session
+  appleAccessory.setNMEApointers(latestGPGGA, latestGPRMC);
+
+  // Pass the pointer for additional NMEA GST, VTG, GSA and GSV data
+  // GGA, RMC, GST, VTG, GSA and GSV are passed to apps over an iAP2 EA Session 
+  appleAccessory.setEASessionPointer(additionalNmeaBuffer);
 
   // Pass the transport connected and disconnect methods into the accessory driver
   appleAccessory.setTransportConnectedMethod(&transportConnected);
@@ -216,6 +258,9 @@ void setup()
 
   //myGNSS.enableDebugging(); // Uncomment this line to enable debug messages on Serial
 
+  // Storage for NMEA GST, VTG, GSA and GSV. setFileBufferSize must be called _before_ .begin
+  myGNSS.setFileBufferSize(additionalNmeaBufferSize);
+
   while (!myGNSS.begin())
   {
     Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring."));
@@ -224,11 +269,11 @@ void setup()
   // Disable or enable various NMEA sentences over the I2C interface
   myGNSS.setI2COutput(COM_TYPE_NMEA | COM_TYPE_UBX); // Turn on both UBX and NMEA sentences on I2C. (Turn off RTCM and SPARTN)
   myGNSS.newCfgValset(VAL_LAYER_RAM_BBR); // Use cfgValset to disable / enable individual NMEA messages
-  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GLL_I2C, 0); // Disable all NMEA messages except GGA and RMC. Enable GST
-  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GSA_I2C, 0);
-  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GSV_I2C, 0);
+  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GLL_I2C, 0); // Enable required NMEA messages
+  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GSA_I2C, 1);
+  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GSV_I2C, 1);
   myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_RMC_I2C, 1);
-  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_VTG_I2C, 0);
+  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_VTG_I2C, 1);
   myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GGA_I2C, 1);
   myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_I2C, 0);
   myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GST_I2C, 1);
@@ -250,9 +295,9 @@ void setup()
   // Set up the callback for GPRMC
   myGNSS.setNMEAGPRMCcallbackPtr(&newGPRMC);
 
-  // Set up the callback for GPGST
-  myGNSS.setNMEAGPGSTcallbackPtr(&newGPGST);
-
+  // Log NMEA GST, VTG, GSA and GSV in the GNSS library file buffer
+  myGNSS.setNMEALoggingMask(SFE_UBLOX_FILTER_NMEA_GST | SFE_UBLOX_FILTER_NMEA_VTG
+                            | SFE_UBLOX_FILTER_NMEA_GSA | SFE_UBLOX_FILTER_NMEA_GSV);
 }
 
 // ===================================================================================================================
@@ -271,20 +316,62 @@ void loop()
   myGNSS.checkCallbacks(); // Check if any callbacks are waiting to be processed.
 
   // ==============================================================================================================
-  // Check for a new device connection
+  // Copy latest GST, VTG, GSA and GSV into additionalNmeaBuffer
 
-  if (SerialBT.aclConnected() == true)
+  // If the Apple Accessory is not sending data to the EA Session, copy the data into additionalNmeaBuffer.
+  // Bad things would happen if we were to manipulate additionalNmeaBuffer while appleAccessory is using it.
+  if (appleAccessory.latestEASessionDataIsBlocking() == false)
   {
-    Serial.println("Apple Device found, connecting...");
+    size_t spaceAvailable = additionalNmeaBufferSize - strlen(additionalNmeaBuffer);
+    if (spaceAvailable >= 1)
+      spaceAvailable -= 1; // Leave room for the NULL
+    uint16_t fileBufferAvailable = myGNSS.fileBufferAvailable(); // Check how much data is available
+    while (spaceAvailable < fileBufferAvailable) // If the buffer is full, delete the oldest message(s)
+    {
+      const char *lfPtr = strstr(additionalNmeaBuffer, "\n"); // Find the first LF
+      if (lfPtr == nullptr)
+        break; // Something has gone badly wrong...
+      lfPtr++; // Point at the byte after the LF
+      size_t oldLen = lfPtr - additionalNmeaBuffer; // This much data is old
+      size_t newLen = strlen(additionalNmeaBuffer) - oldLen; // This much is new (not old)
+      for (size_t i = 0; i <= newLen; i++) // Move the new data over the old. Include the NULL
+        additionalNmeaBuffer[i] = additionalNmeaBuffer[oldLen + i];
+      spaceAvailable += oldLen;
+    }
+    size_t dataLen = strlen(additionalNmeaBuffer);
+    myGNSS.extractFileBufferData((uint8_t *)&additionalNmeaBuffer[dataLen], fileBufferAvailable); // Add the new NMEA data
+    dataLen += fileBufferAvailable;
+    additionalNmeaBuffer[dataLen] = 0; // NULL terminate
+  }
 
-    delay(2);
+  // ==============================================================================================================
+  // Check if the iAP2 SDP record has been created
+  // If it has, restart the SPP Server
 
-    SerialBT.connect(SerialBT.aclGetAddress(), 1); //Connect on channel 1
+  if (sdpCreateRecordEvent)
+  {
+    sdpCreateRecordEvent = false;
+    esp_spp_stop_srv();
+    esp_spp_start_srv(ESP_SPP_SEC_NONE, ESP_SPP_ROLE_SLAVE, rfcommChanneliAP2, "ESP32SPP2");
+  }
+
+  // ==============================================================================================================
+  // Check for a new device connection
+
+  if (SerialBT.aclConnected() == true)
+  {
+    Serial.println("Apple Device found. Waiting for connection...");
 
-    if (SerialBT.connected())
+    unsigned long connectStart = millis();
+    while ((millis() - connectStart) < 5000)
     {
-      Serial.println("Sending handshake...");
-      appleAccessory.startHandshake(&SerialBT);
+      if (SerialBT.connected())
+      {
+        Serial.println("Device connected. Sending handshake...");
+        appleAccessory.startHandshake(&SerialBT);
+        break;
+      }
+      delay(10);
     }
   }