diff --git a/src/BluetoothCommon.h b/src/BluetoothCommon.h index 440d13844..84fa970b6 100644 --- a/src/BluetoothCommon.h +++ b/src/BluetoothCommon.h @@ -14,6 +14,9 @@ #define LEGACY_LOGRADIO_UUID "6c6fd238-78fa-436b-aacf-15c5be1ef2e2" #define LOGRADIO_UUID "5a3d6e49-06e6-4423-9944-e9de8cdf9547" +#define GPWPL_SERVICE_UUID "0000181A-0000-1000-8000-00805f9b34fb"//gpwpl service uuid +#define GPWPL_UUID "00002A6F-0000-1000-8000-00805f9b34fb"//gpwpl uuid + // NRF52 wants these constants as byte arrays // Generated here https://yupana-engineering.com/online-uuid-to-c-array-converter - but in REVERSE BYTE ORDER extern const uint8_t MESH_SERVICE_UUID_16[], TORADIO_UUID_16[16u], FROMRADIO_UUID_16[], FROMNUM_UUID_16[], LOGRADIO_UUID_16[]; diff --git a/src/mesh/NodeDB.cpp b/src/mesh/NodeDB.cpp index 0a79f94a8..6930c38b0 100644 --- a/src/mesh/NodeDB.cpp +++ b/src/mesh/NodeDB.cpp @@ -1479,6 +1479,12 @@ void NodeDB::updatePosition(uint32_t nodeId, const meshtastic_Position &p, RxSou info->has_position = true; updateGUIforNode = info; notifyObservers(true); // Force an update whether or not our node counts have changed + +#if !MESHTASTIC_EXCLUDE_BLUETOOTH +#ifdef ARCH_ESP32 + nimbleBluetooth->Send_GPWPL(nodeId, info->user.short_name, info->position.latitude_i, info->position.longitude_i); +#endif +#endif } /** Update telemetry info for this node based on received metrics diff --git a/src/nimble/NimbleBluetooth.cpp b/src/nimble/NimbleBluetooth.cpp index 009439f25..675c6ccc4 100644 --- a/src/nimble/NimbleBluetooth.cpp +++ b/src/nimble/NimbleBluetooth.cpp @@ -13,6 +13,7 @@ NimBLECharacteristic *fromNumCharacteristic; NimBLECharacteristic *BatteryCharacteristic; NimBLECharacteristic *logRadioCharacteristic; +NimBLECharacteristic *gpwplCharacteristic; NimBLEServer *bleServer; static bool passkeyShowing; @@ -265,6 +266,11 @@ void NimbleBluetooth::setupService() batteryLevelDescriptor->setUnit(0x27ad); batteryService->start(); + + // Setup the gpwpl service + NimBLEService *gpwplService = bleServer->createService(GPWPL_SERVICE_UUID); + gpwplCharacteristic = gpwplService->createCharacteristic(GPWPL_UUID, NIMBLE_PROPERTY::NOTIFY); + gpwplService->start(); } void NimbleBluetooth::startAdvertising() @@ -273,6 +279,7 @@ void NimbleBluetooth::startAdvertising() pAdvertising->reset(); pAdvertising->addServiceUUID(MESH_SERVICE_UUID); pAdvertising->addServiceUUID(NimBLEUUID((uint16_t)0x180f)); // 0x180F is the Battery Service + pAdvertising->addServiceUUID(GPWPL_SERVICE_UUID); pAdvertising->start(0); } @@ -299,6 +306,65 @@ void NimbleBluetooth::sendLog(const uint8_t *logMessage, size_t length) logRadioCharacteristic->notify(logMessage, length, true); } +// Convert the decimal format of latitude and longitude to NMEA format (degrees and minutes) +void convertToNMEAFormat(double value, char* buffer, int bufferSize, int isLatitude) { + int degrees = (int)value; + double minutes = (value - degrees) * 60.0; + + if (isLatitude) { + snprintf(buffer, bufferSize, "%02d%07.4f", degrees, minutes); + } else { + snprintf(buffer, bufferSize, "%03d%07.4f", degrees, minutes); + } +} + +void NimbleBluetooth::Send_GPWPL(uint32_t node, char* name, int32_t latitude_i, int32_t longitude_i) +{ + double f_latitude = (double)latitude_i * (double)0.0000001; + double f_longitude = (double)longitude_i * (double)0.0000001; + char str_latitude_head[20] = {0}; + char str_longitude_head[20] = {0}; + convertToNMEAFormat(fabs(f_latitude), str_latitude_head, sizeof(str_latitude_head), 1); + convertToNMEAFormat(fabs(f_longitude), str_longitude_head, sizeof(str_longitude_head), 0); + + char str_latitude_symbol[2] = {0}; + char str_longitude_symbol[2] = {0}; + str_latitude_symbol[0] = (f_latitude >= 0) ? 'N' : 'S'; + str_longitude_symbol[0] = (f_longitude >= 0) ? 'E' : 'W'; + + char strGPWPL[100] = {0}; + snprintf(strGPWPL, sizeof(strGPWPL), "$GPWPL,%s,%s,%s,%s,%s", + str_latitude_head, str_latitude_symbol, + str_longitude_head, str_longitude_symbol, + name); + + // Calculate the check value + uint8_t checksum = 0; + for (int i = 1; i < strlen(strGPWPL); i++) + { + checksum ^= (uint8_t)strGPWPL[i]; + } + snprintf(strGPWPL + strlen(strGPWPL), sizeof(strGPWPL) - strlen(strGPWPL), "*%02X\r\n\r\n", (uint8_t)checksum); + + // Split the long string and send it. + int nOffset = 0; + int nHaveLen = strlen(strGPWPL); + while (true) + { + int nSendLen = min(20, nHaveLen); + gpwplCharacteristic->setValue(((uint8_t*)strGPWPL) + nOffset, nSendLen); + gpwplCharacteristic->notify(); + delay(50); // To ensure stable transmission, add a little delay. + + nOffset += nSendLen; + nHaveLen -= nSendLen; + if (nHaveLen <= 0) + { + break; + } + } +} + void clearNVS() { NimBLEDevice::deleteAllBonds(); diff --git a/src/nimble/NimbleBluetooth.h b/src/nimble/NimbleBluetooth.h index 45602e088..0ece07306 100644 --- a/src/nimble/NimbleBluetooth.h +++ b/src/nimble/NimbleBluetooth.h @@ -12,6 +12,7 @@ class NimbleBluetooth : BluetoothApi bool isConnected(); int getRssi(); void sendLog(const uint8_t *logMessage, size_t length); + void Send_GPWPL(uint32_t node, char* name,int32_t latitude_i,int32_t longitude_i); private: void setupService();