From 40c3784d5ddf6345b75dd507bbb35027000042a8 Mon Sep 17 00:00:00 2001 From: LegoManACM <41910060+LegoManACM@users.noreply.github.com> Date: Wed, 13 May 2026 17:08:29 -0700 Subject: [PATCH] Working State --- src/helpers/rp2040/SerialBLEInterface.cpp | 70 +++++++++++------------ src/helpers/rp2040/SerialBLEInterface.h | 40 +++++++------ variants/pico_w_dragino_sx1276/target.cpp | 1 + 3 files changed, 57 insertions(+), 54 deletions(-) diff --git a/src/helpers/rp2040/SerialBLEInterface.cpp b/src/helpers/rp2040/SerialBLEInterface.cpp index 169255f64..880605c4d 100644 --- a/src/helpers/rp2040/SerialBLEInterface.cpp +++ b/src/helpers/rp2040/SerialBLEInterface.cpp @@ -22,39 +22,50 @@ void SerialBLEInterface::onDisconnect(BLEServer* p) { } } +void SerialBLEInterface::onWrite(BLECharacteristic* c) { + size_t len = c->valueLen(); + const uint8_t* data = (const uint8_t*)c->valueData(); + Serial.printf("BLE: onWrite len=%d hdr=0x%02x\n", len, len > 0 ? data[0] : 0); + if (len > 0 && len <= MAX_FRAME_SIZE && recv_queue_len < FRAME_QUEUE_SIZE) { + recv_queue[recv_queue_len].len = len; + memcpy(recv_queue[recv_queue_len].buf, data, len); + recv_queue_len++; + } +} + void SerialBLEInterface::shiftSendQueueLeft() { if (send_queue_len > 0) { send_queue_len--; - for (uint8_t i = 0; i < send_queue_len; i++) { - send_queue[i] = send_queue[i + 1]; - } + for (uint8_t i = 0; i < send_queue_len; i++) send_queue[i] = send_queue[i + 1]; } } void SerialBLEInterface::shiftRecvQueueLeft() { if (recv_queue_len > 0) { recv_queue_len--; - for (uint8_t i = 0; i < recv_queue_len; i++) { - recv_queue[i] = recv_queue[i + 1]; - } + for (uint8_t i = 0; i < recv_queue_len; i++) recv_queue[i] = recv_queue[i + 1]; } } void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code) { - (void)pin_code; // TODO: add security/PIN support later + (void)pin_code; char dev_name[48]; snprintf(dev_name, sizeof(dev_name), "%s%s", prefix, name); - BLE.begin(dev_name); + BLE.begin(String(dev_name)); _server = BLE.server(); _server->setCallbacks(this); - _server->addService(&_uart); - _uart.begin(); - _uart.setAutoflush(0); // disable autoflush so we control when data is sent - BLE.setSecurity(BLESecurityNone); + _service = new BLEService(String(SERVICE_UUID)); + _txChar = new BLECharacteristic(String(TX_UUID), BLERead | BLENotify); + _rxChar = new BLECharacteristic(String(RX_UUID), BLEWrite | BLEWriteWithoutResponse); + _rxChar->setCallbacks(this); + + _service->addCharacteristic(_txChar); + _service->addCharacteristic(_rxChar); + _server->addService(_service); enable(); } @@ -74,10 +85,7 @@ void SerialBLEInterface::disable() { size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { if (!_isConnected || len == 0 || len > MAX_FRAME_SIZE) return 0; - if (send_queue_len >= FRAME_QUEUE_SIZE) { - BLE_DEBUG_PRINTLN("send queue full"); - return 0; - } + if (send_queue_len >= FRAME_QUEUE_SIZE) return 0; send_queue[send_queue_len].len = len; memcpy(send_queue[send_queue_len].buf, src, len); send_queue_len++; @@ -85,33 +93,19 @@ size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { } size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) { - // drain send queue - if (send_queue_len > 0 && _isConnected) { - Frame& f = send_queue[0]; - size_t written = _uart.write(f.buf, f.len); - _uart.flush(); - Serial.printf("BLE: wrote %d of %d bytes\n", written, f.len); - if (written == f.len) { - shiftSendQueueLeft(); - } - } - - // read incoming - int avail = _uart.available(); - if (avail > 0) { - Serial.printf("BLE: %d bytes available\n", avail); - if (recv_queue_len < FRAME_QUEUE_SIZE) { - if (avail > MAX_FRAME_SIZE) avail = MAX_FRAME_SIZE; - recv_queue[recv_queue_len].len = avail; - _uart.readBytes(recv_queue[recv_queue_len].buf, avail); - recv_queue_len++; - } + // send queued frames + if (send_queue_len > 0 && _isConnected && millis() >= _last_write + BLE_WRITE_MIN_INTERVAL) { + _last_write = millis(); + _txChar->setValue(send_queue[0].buf, send_queue[0].len); + Serial.printf("BLE: notify len=%d hdr=0x%02x\n", send_queue[0].len, send_queue[0].buf[0]); + shiftSendQueueLeft(); } + // return next received frame if (recv_queue_len > 0) { size_t len = recv_queue[0].len; - Serial.printf("BLE: returning frame len=%d hdr=0x%02x\n", len, recv_queue[0].buf[0]); memcpy(dest, recv_queue[0].buf, len); + Serial.printf("BLE: recv frame len=%d hdr=0x%02x\n", len, dest[0]); shiftRecvQueueLeft(); return len; } diff --git a/src/helpers/rp2040/SerialBLEInterface.h b/src/helpers/rp2040/SerialBLEInterface.h index 79c1bdbc1..eb2f4bf95 100644 --- a/src/helpers/rp2040/SerialBLEInterface.h +++ b/src/helpers/rp2040/SerialBLEInterface.h @@ -3,25 +3,22 @@ #include "../BaseSerialInterface.h" #include #include -#include +#include +#include -#ifndef BLE_TX_POWER -#define BLE_TX_POWER 4 -#endif - -#if BLE_DEBUG_LOGGING && ARDUINO - #include - #define BLE_DEBUG_PRINTLN(F, ...) Serial.printf("BLE: " F "\n", ##__VA_ARGS__) -#else - #define BLE_DEBUG_PRINTLN(...) {} -#endif - -class SerialBLEInterface : public BaseSerialInterface, public BLEServerCallbacks { +class SerialBLEInterface : public BaseSerialInterface, public BLEServerCallbacks, public BLECharacteristicCallbacks { BLEServer* _server = nullptr; - BLEServiceUART _uart; + BLEService* _service = nullptr; + BLECharacteristic* _txChar = new BLECharacteristic(String(TX_UUID), BLERead | BLENotify); + BLECharacteristic* _rxChar = _rxChar = new BLECharacteristic(String(RX_UUID), BLEWrite | BLEWriteWithoutResponse); bool _isEnabled = false; bool _isConnected = false; unsigned long _last_health_check = 0; + unsigned long _last_write = 0; + + static constexpr const char* SERVICE_UUID = "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"; + static constexpr const char* TX_UUID = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"; + static constexpr const char* RX_UUID = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"; struct Frame { uint8_t len; @@ -29,6 +26,7 @@ class SerialBLEInterface : public BaseSerialInterface, public BLEServerCallbacks }; #define FRAME_QUEUE_SIZE 12 + #define BLE_WRITE_MIN_INTERVAL 60 uint8_t send_queue_len = 0; Frame send_queue[FRAME_QUEUE_SIZE]; @@ -48,11 +46,21 @@ public: void onConnect(BLEServer* p) override; void onDisconnect(BLEServer* p) override; + // BLECharacteristicCallbacks - called when app writes to RX characteristic + void onWrite(BLECharacteristic* c) override; + void enable() override; void disable() override; bool isEnabled() const override { return _isEnabled; } bool isConnected() const override { return _isConnected; } - bool isWriteBusy() const override { return send_queue_len >= (FRAME_QUEUE_SIZE * 2 / 3); } + bool isWriteBusy() const override { return millis() < _last_write + BLE_WRITE_MIN_INTERVAL; } size_t writeFrame(const uint8_t src[], size_t len) override; size_t checkRecvFrame(uint8_t dest[]) override; -}; \ No newline at end of file +}; + +#if BLE_DEBUG_LOGGING && ARDUINO + #include + #define BLE_DEBUG_PRINTLN(F, ...) Serial.printf("BLE: " F "\n", ##__VA_ARGS__) +#else + #define BLE_DEBUG_PRINTLN(...) {} +#endif \ No newline at end of file diff --git a/variants/pico_w_dragino_sx1276/target.cpp b/variants/pico_w_dragino_sx1276/target.cpp index b2b65182f..aec34ed78 100644 --- a/variants/pico_w_dragino_sx1276/target.cpp +++ b/variants/pico_w_dragino_sx1276/target.cpp @@ -66,6 +66,7 @@ void radio_set_params(float freq, float bw, uint8_t sf, uint8_t cr) { } void radio_set_tx_power(int8_t dbm) { + Serial.printf("TX power set: %d dBm\n", dbm); radio.setOutputPower(dbm); }