From a75a93d6a03eb07ccbc8319f35bfed1ec757881c Mon Sep 17 00:00:00 2001 From: burnsed Date: Sat, 9 Jul 2022 15:38:11 -0700 Subject: [PATCH] working BLE support --- Firmware/RTK_Surveyor/Begin.ino | 2 +- Firmware/RTK_Surveyor/NVM.ino | 3 + Firmware/RTK_Surveyor/RTK_Surveyor.ino | 12 +- Firmware/RTK_Surveyor/System.ino | 22 ++- Firmware/RTK_Surveyor/Tasks.ino | 10 +- Firmware/RTK_Surveyor/btSelect.h | 161 +++++++++++++++ Firmware/RTK_Surveyor/menuBluetooth.ino | 45 +++++ Firmware/RTK_Surveyor/menuMain.ino | 4 + Firmware/RTK_Surveyor/settings.h | 2 + .../RTK_Surveyor/src/BleSerial/BleSerial.cpp | 184 ++++++++++++++++++ .../RTK_Surveyor/src/BleSerial/BleSerial.h | 76 ++++++++ .../src/BleSerial/ByteRingBuffer.h | 45 +++++ 12 files changed, 551 insertions(+), 15 deletions(-) create mode 100644 Firmware/RTK_Surveyor/btSelect.h create mode 100644 Firmware/RTK_Surveyor/menuBluetooth.ino create mode 100644 Firmware/RTK_Surveyor/src/BleSerial/BleSerial.cpp create mode 100644 Firmware/RTK_Surveyor/src/BleSerial/BleSerial.h create mode 100644 Firmware/RTK_Surveyor/src/BleSerial/ByteRingBuffer.h diff --git a/Firmware/RTK_Surveyor/Begin.ino b/Firmware/RTK_Surveyor/Begin.ino index e3c5b9fb1..7234df276 100644 --- a/Firmware/RTK_Surveyor/Begin.ino +++ b/Firmware/RTK_Surveyor/Begin.ino @@ -298,7 +298,7 @@ void pinUART2Task( void *pvParameters ) vTaskDelete( NULL ); //Delete task once it has run once } -//Serial Read/Write tasks for the F9P must be started after BT is up and running otherwise SerialBT.available will cause reboot +//Serial Read/Write tasks for the F9P must be started after BT is up and running otherwise SerialBT->available will cause reboot void startUART2Tasks() { //Start the tasks for handling incoming and outgoing BT bytes to/from ZED-F9P diff --git a/Firmware/RTK_Surveyor/NVM.ino b/Firmware/RTK_Surveyor/NVM.ino index 563416e57..b997cc06a 100644 --- a/Firmware/RTK_Surveyor/NVM.ino +++ b/Firmware/RTK_Surveyor/NVM.ino @@ -203,6 +203,7 @@ void recordSystemSettingsToFile(File * settingsFile) settingsFile->printf("%s=%llu\n\r", F("lastKeyAttempt"), settings.lastKeyAttempt); settingsFile->printf("%s=%d\n\r", F("updateZEDSettings"), settings.updateZEDSettings); settingsFile->printf("%s=%d\n\r", F("LBandFreq"), settings.LBandFreq); + settingsFile->printf("%s=%d\n\r", F("enableBLE"), settings.enableBLE); //Record constellation settings for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++) @@ -733,6 +734,8 @@ bool parseLine(char* str, Settings *settings) } else if (strcmp(settingName, "LBandFreq") == 0) settings->LBandFreq = d; + else if (strcmp(settingName, "enableBLE") == 0) + settings->enableBLE = d; //Check for bulk settings (constellations and message rates) //Must be last on else list diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino index 917d2340a..797c7875d 100644 --- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino +++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino @@ -231,9 +231,17 @@ float battChangeRate = 0.0; //Hardware serial and BT buffers //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= #ifdef COMPILE_BT +// See btSelect.h for implemenation +#include "btSelect.h" +BTSerialInterface * SerialBT; + //We use a local copy of the BluetoothSerial library so that we can increase the RX buffer. See issue: https://github.com/sparkfun/SparkFun_RTK_Firmware/issues/23 -#include "src/BluetoothSerial/BluetoothSerial.h" -BluetoothSerial SerialBT; +// #include "src/BluetoothSerial/BluetoothSerial.h" +// BluetoothSerial SerialBT; + +// BLE Support originally from https://github.com/avinabmalla/ESP32_BleSerial/tree/bad5ff841800853a61e431ea751f8ea9d7a1df21 +// #include "src/BleSerial/BleSerial.h" +// BleSerial SerialBLE; #endif char platformPrefix[40] = "Surveyor"; //Sets the prefix for broadcast names diff --git a/Firmware/RTK_Surveyor/System.ino b/Firmware/RTK_Surveyor/System.ino index 97a8468c4..f78566fc1 100644 --- a/Firmware/RTK_Surveyor/System.ino +++ b/Firmware/RTK_Surveyor/System.ino @@ -14,7 +14,15 @@ void startBluetooth() sprintf(deviceName, "%s %s-%02X%02X", platformPrefix, stateName, unitMACAddress[4], unitMACAddress[5]); //Base mode - if (SerialBT.begin(deviceName, false, settings.sppRxQueueSize, settings.sppTxQueueSize) == false) //localName, isMaster, rxBufferSize, txBufferSize + // BLE vs Bluetooth Classic + if (settings.enableBLE) + SerialBT = new BTLESerial(); + else + { + SerialBT = new BTClassicSerial(); + } + + if (SerialBT->begin(deviceName, false, settings.sppRxQueueSize, settings.sppTxQueueSize) == false) //localName, isMaster, rxBufferSize, txBufferSize { Serial.println(F("An error occurred initializing Bluetooth")); @@ -43,8 +51,8 @@ void startBluetooth() esp_bt_gap_set_pin(pin_type, 4, pin_code); //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- - SerialBT.register_callback(btCallback); //Controls BT Status LED on Surveyor - SerialBT.setTimeout(250); + SerialBT->register_callback(btCallback); //Controls BT Status LED on Surveyor + SerialBT->setTimeout(250); Serial.print(F("Bluetooth broadcasting as: ")); Serial.println(deviceName); @@ -71,10 +79,10 @@ void stopBluetooth() if (btState == BT_NOTCONNECTED || btState == BT_CONNECTED) { #ifdef COMPILE_BT - SerialBT.register_callback(NULL); - SerialBT.flush(); //Complete any transfers - SerialBT.disconnect(); //Drop any clients - SerialBT.end(); //SerialBT.end() will release significant RAM (~100k!) but a SerialBT.start will crash. + SerialBT->register_callback(NULL); + SerialBT->flush(); //Complete any transfers + SerialBT->disconnect(); //Drop any clients + SerialBT->end(); //SerialBT->end() will release significant RAM (~100k!) but a SerialBT->start will crash. #endif log_d("Bluetooth turned off"); diff --git a/Firmware/RTK_Surveyor/Tasks.ino b/Firmware/RTK_Surveyor/Tasks.ino index f9bfa3268..695513cd7 100644 --- a/Firmware/RTK_Surveyor/Tasks.ino +++ b/Firmware/RTK_Surveyor/Tasks.ino @@ -11,10 +11,10 @@ void F9PSerialWriteTask(void *e) //Receive RTCM corrections or UBX config messages over bluetooth and pass along to ZED if (btState == BT_CONNECTED) { - while (SerialBT.available()) + while (SerialBT->available()) { //Pass bytes to GNSS receiver - auto s = SerialBT.readBytes(wBuffer, sizeof(wBuffer)); + auto s = SerialBT->readBytes(wBuffer, sizeof(wBuffer)); serialGNSS.write(wBuffer, s); if (settings.enableTaskReports == true) @@ -49,13 +49,13 @@ void F9PSerialReadTask(void *e) #ifdef COMPILE_BT else if (btState == BT_CONNECTED) { - if (SerialBT.isCongested() == false) + if (SerialBT->isCongested() == false) { - SerialBT.write(rBuffer, s); //Push new data to BT SPP + SerialBT->write(rBuffer, s); //Push new data to BT SPP } else if (settings.throttleDuringSPPCongestion == false) { - SerialBT.write(rBuffer, s); //Push new data to SPP regardless of congestion + SerialBT->write(rBuffer, s); //Push new data to SPP regardless of congestion } else { diff --git a/Firmware/RTK_Surveyor/btSelect.h b/Firmware/RTK_Surveyor/btSelect.h new file mode 100644 index 000000000..d41e34e58 --- /dev/null +++ b/Firmware/RTK_Surveyor/btSelect.h @@ -0,0 +1,161 @@ +#ifdef COMPILE_BT + +#include "src/BluetoothSerial/BluetoothSerial.h" +#include "src/BleSerial/BleSerial.h" + +class BTSerialInterface +{ + public: + virtual bool begin(String deviceName, bool isMaster, uint16_t rxQueueSize, uint16_t txQueueSize) = 0; + virtual void disconnect() = 0; + virtual void end() = 0; + virtual esp_err_t register_callback(esp_spp_cb_t * callback) = 0; + virtual void setTimeout(unsigned long timeout) = 0; + + virtual int available() = 0; + virtual size_t readBytes(uint8_t *buffer, size_t bufferSize) = 0; + + virtual bool isCongested() = 0; + virtual size_t write(const uint8_t *buffer, size_t size) = 0; + virtual void flush() = 0; +}; + + +class BTClassicSerial : public virtual BTSerialInterface, public BluetoothSerial +{ + // Everything is already implemented in BluetoothSerial since the code was + // originally written using that class + public: + bool begin(String deviceName, bool isMaster, uint16_t rxQueueSize, uint16_t txQueueSize) + { + return BluetoothSerial::begin(deviceName, isMaster, rxQueueSize, txQueueSize); + } + + void disconnect() + { + BluetoothSerial::disconnect(); + } + + void end() + { + BluetoothSerial::end(); + } + + esp_err_t register_callback(esp_spp_cb_t * callback) + { + return BluetoothSerial::register_callback(callback); + } + + void setTimeout(unsigned long timeout) + { + BluetoothSerial::setTimeout(timeout); + } + + int available() + { + return BluetoothSerial::available(); + } + + size_t readBytes(uint8_t *buffer, size_t bufferSize) + { + return BluetoothSerial::readBytes(buffer, bufferSize); + } + + bool isCongested() + { + return BluetoothSerial::isCongested(); + } + + size_t write(const uint8_t *buffer, size_t size) + { + return BluetoothSerial::write(buffer, size); + } + + void flush() + { + BluetoothSerial::flush(); + } +}; + + +class BTLESerial: public virtual BTSerialInterface, public BleSerial +{ + public: + // Missing from BleSerial + bool begin(String deviceName, bool isMaster, uint16_t rxQueueSuze, uint16_t txQueueSize) + { + // Curretnly ignoring rxQueueSize + // transmitBufferLength = txQueueSize; + BleSerial::begin(deviceName.c_str()); + return true; + } + + void disconnect() + { + Server->disconnect(Server->getConnId()); + } + + void end() + { + BleSerial::end(); + } + + esp_err_t register_callback(esp_spp_cb_t * callback) + { + connectionCallback = callback; + return ESP_OK; + } + + void setTimeout(unsigned long timeout) + { + BleSerial::setTimeout(timeout); + } + + int available() + { + return BleSerial::available(); + } + + size_t readBytes(uint8_t *buffer, size_t bufferSize) + { + return BleSerial::readBytes(buffer, bufferSize); + } + + bool isCongested() + { + // not currently supported in this implementation + return false; + } + + size_t write(const uint8_t *buffer, size_t size) + { + return BleSerial::write(buffer, size); + } + + void flush() + { + BleSerial::flush(); + } + + // override BLEServerCallbacks + void onConnect(BLEServer *pServer) + { + bleConnected = true; + connectionCallback(ESP_SPP_SRV_OPEN_EVT, nullptr); + } + + void onDisconnect(BLEServer *pServer) + { + bleConnected = false; + connectionCallback(ESP_SPP_CLOSE_EVT, nullptr); + Server->startAdvertising(); + } + + private: + esp_spp_cb_t * connectionCallback; + +}; + + + +#endif diff --git a/Firmware/RTK_Surveyor/menuBluetooth.ino b/Firmware/RTK_Surveyor/menuBluetooth.ino new file mode 100644 index 000000000..cbe8012fc --- /dev/null +++ b/Firmware/RTK_Surveyor/menuBluetooth.ino @@ -0,0 +1,45 @@ +void menuBluetooth() +{ + while (1) + { + Serial.println(); + Serial.println(F("Menu: Blueooth Menu")); + + Serial.println(); + Serial.print(F("Current Bluetooth Mode: ")); + if (settings.enableBLE == true) + Serial.println(F("BLE")); + else + Serial.println(F("Classic")); + + Serial.println(); + Serial.println(F("1) Set Bluetooth Mode to Classic")); + Serial.println(F("2) Set Bluetooth Mode to BLE")); + Serial.println(F("x) Exit")); + + byte incoming = getByteChoice(menuTimeout); + if (incoming == '1') + { + // Restart Bluetooth + stopBluetooth(); + settings.enableBLE = false; + startBluetooth(); + } + else if (incoming == '2') + { + // restart Bluetooth + stopBluetooth(); + settings.enableBLE = true; + startBluetooth(); + } + else if (incoming == 'x') + break; + else if (incoming == STATUS_GETBYTE_TIMEOUT) + break; + else + printUnknown(incoming); + } + + while (Serial.available()) Serial.read(); +} + diff --git a/Firmware/RTK_Surveyor/menuMain.ino b/Firmware/RTK_Surveyor/menuMain.ino index edebab55a..86b0768d2 100644 --- a/Firmware/RTK_Surveyor/menuMain.ino +++ b/Firmware/RTK_Surveyor/menuMain.ino @@ -46,6 +46,8 @@ void menuMain() Serial.println(F("5) Configure Logging")); + Serial.println(F("6) Configure Bluetooth")); + Serial.println(F("p) Configure Profiles")); if (online.lband == true) @@ -72,6 +74,8 @@ void menuMain() menuPorts(); else if (incoming == '5') menuLog(); + else if (incoming == '6') + menuBluetooth(); else if (incoming == 's') menuSystem(); else if (incoming == 'p') diff --git a/Firmware/RTK_Surveyor/settings.h b/Firmware/RTK_Surveyor/settings.h index f2c29f700..506a059d7 100644 --- a/Firmware/RTK_Surveyor/settings.h +++ b/Firmware/RTK_Surveyor/settings.h @@ -224,6 +224,8 @@ typedef struct { pulseEdgeType_e externalPulsePolarity = PULSE_RISING_EDGE; //Pulse rises for pulse length, then falls bool enableExternalHardwareEventLogging = false; //Log when INT/TM2 pin goes low + bool enableBLE = false; + ubxMsg ubxMessages[MAX_UBX_MSG] = //Report rates for all known messages { //NMEA diff --git a/Firmware/RTK_Surveyor/src/BleSerial/BleSerial.cpp b/Firmware/RTK_Surveyor/src/BleSerial/BleSerial.cpp new file mode 100644 index 000000000..bcb5cb620 --- /dev/null +++ b/Firmware/RTK_Surveyor/src/BleSerial/BleSerial.cpp @@ -0,0 +1,184 @@ +#include "BleSerial.h" +using namespace std; + +bool BleSerial::connected() +{ + return Server->getConnectedCount() > 0; +} + +void BleSerial::onConnect(BLEServer *pServer) +{ + bleConnected = true; + if(ENABLE_LED) digitalWrite(BLUE_LED, HIGH); +} + +void BleSerial::onDisconnect(BLEServer *pServer) +{ + bleConnected = false; + if(ENABLE_LED) digitalWrite(BLUE_LED, LOW); + Server->startAdvertising(); +} + +int BleSerial::read() +{ + uint8_t result = this->receiveBuffer.pop(); + if (result == (uint8_t)'\n') + { + this->numAvailableLines--; + } + return result; +} + +size_t BleSerial::readBytes(uint8_t *buffer, size_t bufferSize) +{ + int i = 0; + while (i < bufferSize && available()) + { + buffer[i] = this->receiveBuffer.pop(); + i++; + } + return i; +} + +int BleSerial::peek() +{ + if (this->receiveBuffer.getLength() == 0) + return -1; + return this->receiveBuffer.get(0); +} + +int BleSerial::available() +{ + return this->receiveBuffer.getLength(); +} + +size_t BleSerial::print(const char *str) +{ + if (Server->getConnectedCount() <= 0) + { + return 0; + } + + size_t written = 0; + for (size_t i = 0; str[i] != '\0'; i++) + { + written += this->write(str[i]); + } + flush(); + return written; +} + +size_t BleSerial::write(const uint8_t *buffer, size_t bufferSize) +{ + if (Server->getConnectedCount() <= 0) + { + return 0; + } + size_t written = 0; + for (int i = 0; i < bufferSize; i++) + { + written += this->write(buffer[i]); + } + flush(); + return written; +} + +size_t BleSerial::write(uint8_t byte) +{ + if (Server->getConnectedCount() <= 0) + { + return 0; + } + this->transmitBuffer[this->transmitBufferLength] = byte; + this->transmitBufferLength++; + if (this->transmitBufferLength == sizeof(this->transmitBuffer)) + { + flush(); + } + return 1; +} + +void BleSerial::flush() +{ + if (this->transmitBufferLength > 0) + { + TxCharacteristic->setValue(this->transmitBuffer, this->transmitBufferLength); + this->transmitBufferLength = 0; + } + this->lastFlushTime = millis(); + TxCharacteristic->notify(true); +} + +void BleSerial::begin(const char *name) +{ + //characteristic property is what the other device does. + + ConnectedDeviceCount = 0; + BLEDevice::init(name); + //BLEDevice::setEncryptionLevel(ESP_BLE_SEC_ENCRYPT); + + Server = BLEDevice::createServer(); + Server->setCallbacks(this); + + SetupSerialService(); + + pAdvertising = BLEDevice::getAdvertising(); + pAdvertising->addServiceUUID(BLE_SERIAL_SERVICE_UUID); + pAdvertising->setScanResponse(true); + pAdvertising->setMinPreferred(0x06); // functions that help with iPhone connections issue + pAdvertising->setMinPreferred(0x12); + pAdvertising->start(); + + //pSecurity = new BLESecurity(); + + //Set static pin + //uint32_t passkey = 123456; + //esp_ble_gap_set_security_param(ESP_BLE_SM_SET_STATIC_PASSKEY, &passkey, sizeof(uint32_t)); + //pSecurity->setCapability(ESP_IO_CAP_OUT); +} + +void BleSerial::end() +{ + BLEDevice::deinit(); +} + +void BleSerial::onWrite(BLECharacteristic *pCharacteristic) +{ + if (pCharacteristic->getUUID().toString() == BLE_RX_UUID) + { + std::string value = pCharacteristic->getValue(); + + if (value.length() > 0) + { + for (int i = 0; i < value.length(); i++) + receiveBuffer.add(value[i]); + } + } +} + +void BleSerial::SetupSerialService() +{ + SerialService = Server->createService(BLE_SERIAL_SERVICE_UUID); + + RxCharacteristic = SerialService->createCharacteristic( + BLE_RX_UUID, BLECharacteristic::PROPERTY_WRITE); + + TxCharacteristic = SerialService->createCharacteristic( + BLE_TX_UUID, BLECharacteristic::PROPERTY_NOTIFY); + + TxCharacteristic->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED); + RxCharacteristic->setAccessPermissions(ESP_GATT_PERM_WRITE_ENCRYPTED); + + TxCharacteristic->addDescriptor(new BLE2902()); + RxCharacteristic->addDescriptor(new BLE2902()); + + TxCharacteristic->setReadProperty(true); + RxCharacteristic->setWriteProperty(true); + RxCharacteristic->setCallbacks(this); + SerialService->start(); +} + +BleSerial::BleSerial() +{ +} + diff --git a/Firmware/RTK_Surveyor/src/BleSerial/BleSerial.h b/Firmware/RTK_Surveyor/src/BleSerial/BleSerial.h new file mode 100644 index 000000000..13d4cd3cd --- /dev/null +++ b/Firmware/RTK_Surveyor/src/BleSerial/BleSerial.h @@ -0,0 +1,76 @@ +#pragma once +#include + +#include +#include +#include +#include +#include "ByteRingBuffer.h" + +//Connection status LED +#define ENABLE_LED 0 +#define BLUE_LED 13 + +#define BLE_BUFFER_SIZE 500 //must be greater than MTU, less than ESP_GATT_MAX_ATTR_LEN + + + +class BleSerial : public BLECharacteristicCallbacks, public BLEServerCallbacks, public Stream +{ +public: + BleSerial(); + + void begin(const char *name); + void end(); + void onWrite(BLECharacteristic *pCharacteristic); + int available(); + int read(); + size_t readBytes(uint8_t *buffer, size_t bufferSize); + int peek(); + size_t write(uint8_t byte); + void flush(); + size_t write(const uint8_t *buffer, size_t bufferSize); + size_t print(const char *value); + void onConnect(BLEServer *pServer); + void onDisconnect(BLEServer *pServer); + + bool connected(); + + BLEServer *Server; + + BLEAdvertising *pAdvertising; + //BLESecurity *pSecurity; + + //Services + BLEService *SerialService; + + //Serial Characteristics + BLECharacteristic *TxCharacteristic; + BLECharacteristic *RxCharacteristic; + +protected: + size_t transmitBufferLength; + bool bleConnected; + +private: + BleSerial(BleSerial const &other) = delete; // disable copy constructor + void operator=(BleSerial const &other) = delete; // disable assign constructor + + ByteRingBuffer receiveBuffer; + size_t numAvailableLines; + + unsigned long long lastFlushTime; + uint8_t transmitBuffer[BLE_BUFFER_SIZE]; + + int ConnectedDeviceCount; + void SetupSerialService(); + + /* + Bluetooth LE GATT UUIDs for the Nordic UART profile + Change UUID here if required + */ + const char *BLE_SERIAL_SERVICE_UUID = "6e400001-b5a3-f393-e0a9-e50e24dcca9e"; + const char *BLE_RX_UUID = "6e400002-b5a3-f393-e0a9-e50e24dcca9e"; + const char *BLE_TX_UUID = "6e400003-b5a3-f393-e0a9-e50e24dcca9e"; +}; + diff --git a/Firmware/RTK_Surveyor/src/BleSerial/ByteRingBuffer.h b/Firmware/RTK_Surveyor/src/BleSerial/ByteRingBuffer.h new file mode 100644 index 000000000..aaa81184a --- /dev/null +++ b/Firmware/RTK_Surveyor/src/BleSerial/ByteRingBuffer.h @@ -0,0 +1,45 @@ +#pragma once +#include + + +template +class ByteRingBuffer +{ +private: + uint8_t ringBuffer[N]; + size_t newestIndex = 0; + size_t length = 0; + +public: + void add(uint8_t value) + { + ringBuffer[newestIndex] = value; + newestIndex = (newestIndex + 1) % N; + length = min(length + 1, N); + } + uint8_t pop() + { // pops the oldest value off the ring buffer + if (length == 0) + { + return -1; + } + uint8_t result = ringBuffer[(N + newestIndex - length) % N]; + length -= 1; + return result; + } + void clear() + { + newestIndex = 0; + length = 0; + } + uint8_t get(size_t index) + { // this.get(0) is the oldest value, this.get(this.getLength() - 1) is the newest value + if (index < 0 || index >= length) + { + return -1; + } + return ringBuffer[(N + newestIndex - length + index) % N]; + } + size_t getLength() { return length; } +}; +