Improve SerialBLEInterface

This commit is contained in:
liquidraver 2025-12-04 11:48:01 +01:00
parent 1a3f7a7ea9
commit 73ab0d8813
2 changed files with 289 additions and 119 deletions

View file

@ -1,193 +1,353 @@
#include "SerialBLEInterface.h"
#include <stdio.h>
#include <string.h>
#include "ble_gap.h"
#include "ble_hci.h"
static SerialBLEInterface* instance;
#define BLE_HEALTH_CHECK_INTERVAL 10000 // Advertising watchdog check every 10 seconds
static SerialBLEInterface* instance = nullptr;
void SerialBLEInterface::onConnect(uint16_t connection_handle) {
BLE_DEBUG_PRINTLN("SerialBLEInterface: connected");
// we now set _isDeviceConnected=true in onSecured callback instead
BLE_DEBUG_PRINTLN("SerialBLEInterface: connected handle=0x%04X", connection_handle);
if (instance) {
instance->_conn_handle = connection_handle;
instance->_isDeviceConnected = false;
instance->clearBuffers();
}
}
void SerialBLEInterface::onDisconnect(uint16_t connection_handle, uint8_t reason) {
BLE_DEBUG_PRINTLN("SerialBLEInterface: disconnected reason=%d", reason);
if(instance){
instance->_isDeviceConnected = false;
instance->startAdv();
BLE_DEBUG_PRINTLN("SerialBLEInterface: disconnected handle=0x%04X reason=%u", connection_handle, reason);
if (instance) {
if (instance->_conn_handle == connection_handle) {
instance->_conn_handle = BLE_CONN_HANDLE_INVALID;
instance->_isDeviceConnected = false;
instance->clearBuffers();
}
}
}
void SerialBLEInterface::onSecured(uint16_t connection_handle) {
BLE_DEBUG_PRINTLN("SerialBLEInterface: onSecured");
if(instance){
instance->_isDeviceConnected = true;
// no need to stop advertising on connect, as the ble stack does this automatically
BLE_DEBUG_PRINTLN("SerialBLEInterface: onSecured handle=0x%04X", connection_handle);
if (instance) {
if (instance->isValidConnection(connection_handle, true)) {
instance->_isDeviceConnected = true;
// Connection interval units: 1.25ms, supervision timeout units: 10ms
// Apple: "The product will not read or use the parameters in the Peripheral Preferred Connection Parameters characteristic."
// So we explicitly set it here to make Android & Apple match
ble_gap_conn_params_t conn_params;
conn_params.min_conn_interval = 12; // 15ms
conn_params.max_conn_interval = 24; // 30ms
conn_params.slave_latency = 0;
conn_params.conn_sup_timeout = 200; // 2000ms
uint32_t err_code = sd_ble_gap_conn_param_update(connection_handle, &conn_params);
if (err_code == NRF_SUCCESS) {
BLE_DEBUG_PRINTLN("Connection parameter update requested: 15-30ms interval, 2s timeout");
} else {
BLE_DEBUG_PRINTLN("Failed to request connection parameter update: %lu", err_code);
}
} else {
BLE_DEBUG_PRINTLN("onSecured: ignoring stale/duplicate callback");
}
}
}
bool SerialBLEInterface::onPairingPasskey(uint16_t connection_handle, uint8_t const passkey[6], bool match_request) {
(void)connection_handle;
(void)passkey;
BLE_DEBUG_PRINTLN("SerialBLEInterface: pairing passkey request match=%d", match_request);
return true;
}
void SerialBLEInterface::onPairingComplete(uint16_t connection_handle, uint8_t auth_status) {
BLE_DEBUG_PRINTLN("SerialBLEInterface: pairing complete handle=0x%04X status=%u", connection_handle, auth_status);
if (instance) {
if (instance->isValidConnection(connection_handle)) {
if (auth_status == BLE_GAP_SEC_STATUS_SUCCESS) {
BLE_DEBUG_PRINTLN("SerialBLEInterface: pairing successful");
} else {
BLE_DEBUG_PRINTLN("SerialBLEInterface: pairing failed, disconnecting");
instance->disconnect();
}
} else {
BLE_DEBUG_PRINTLN("onPairingComplete: ignoring stale callback");
}
}
}
void SerialBLEInterface::onBLEEvent(ble_evt_t* evt) {
if (!instance) return;
if (evt->header.evt_id == BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST) {
uint16_t conn_handle = evt->evt.gap_evt.conn_handle;
if (instance->isValidConnection(conn_handle)) {
BLE_DEBUG_PRINTLN("CONN_PARAM_UPDATE_REQUEST: handle=0x%04X, min_interval=%u, max_interval=%u, latency=%u, timeout=%u",
conn_handle,
evt->evt.gap_evt.params.conn_param_update_request.conn_params.min_conn_interval,
evt->evt.gap_evt.params.conn_param_update_request.conn_params.max_conn_interval,
evt->evt.gap_evt.params.conn_param_update_request.conn_params.slave_latency,
evt->evt.gap_evt.params.conn_param_update_request.conn_params.conn_sup_timeout);
uint32_t err_code = sd_ble_gap_conn_param_update(conn_handle, NULL);
if (err_code == NRF_SUCCESS) {
BLE_DEBUG_PRINTLN("Accepted CONN_PARAM_UPDATE_REQUEST (using PPCP)");
} else {
BLE_DEBUG_PRINTLN("ERROR: Failed to accept CONN_PARAM_UPDATE_REQUEST: 0x%08X", err_code);
}
} else {
BLE_DEBUG_PRINTLN("CONN_PARAM_UPDATE_REQUEST: ignoring stale callback for handle=0x%04X", conn_handle);
}
}
}
void SerialBLEInterface::begin(const char* device_name, uint32_t pin_code) {
instance = this;
char charpin[20];
sprintf(charpin, "%d", pin_code);
snprintf(charpin, sizeof(charpin), "%lu", (unsigned long)pin_code);
// If we want to control BLE LED ourselves, uncomment this:
// Bluefruit.autoConnLed(false);
Bluefruit.configPrphBandwidth(BANDWIDTH_MAX);
Bluefruit.configPrphConn(250, BLE_GAP_EVENT_LENGTH_MIN, 16, 16); // increase MTU
Bluefruit.setTxPower(BLE_TX_POWER);
Bluefruit.begin();
// Connection interval units: 1.25ms, supervision timeout units: 10ms
ble_gap_conn_params_t ppcp_params;
ppcp_params.min_conn_interval = 12; // 15ms
ppcp_params.max_conn_interval = 24; // 30ms
ppcp_params.slave_latency = 0;
ppcp_params.conn_sup_timeout = 200; // 2000ms
uint32_t err_code = sd_ble_gap_ppcp_set(&ppcp_params);
if (err_code == NRF_SUCCESS) {
BLE_DEBUG_PRINTLN("PPCP set: 15-30ms interval, 2s timeout");
} else {
BLE_DEBUG_PRINTLN("Failed to set PPCP: %lu", err_code);
}
Bluefruit.setTxPower(BLE_TX_POWER);
Bluefruit.setName(device_name);
Bluefruit.Security.setMITM(true);
Bluefruit.Security.setPIN(charpin);
Bluefruit.Security.setIOCaps(true, false, false);
Bluefruit.Security.setPairPasskeyCallback(onPairingPasskey);
Bluefruit.Security.setPairCompleteCallback(onPairingComplete);
Bluefruit.Periph.setConnectCallback(onConnect);
Bluefruit.Periph.setDisconnectCallback(onDisconnect);
Bluefruit.Security.setSecuredCallback(onSecured);
// To be consistent OTA DFU should be added first if it exists
//bledfu.begin();
Bluefruit.setEventCallback(onBLEEvent);
// Configure and start the BLE Uart service
bleuart.setPermission(SECMODE_ENC_WITH_MITM, SECMODE_ENC_WITH_MITM);
bleuart.begin();
}
bleuart.setRxCallback(onBleUartRX);
void SerialBLEInterface::startAdv() {
BLE_DEBUG_PRINTLN("SerialBLEInterface: starting advertising");
// clean restart if already advertising
if(Bluefruit.Advertising.isRunning()){
BLE_DEBUG_PRINTLN("SerialBLEInterface: already advertising, stopping to allow clean restart");
Bluefruit.Advertising.stop();
}
Bluefruit.Advertising.clearData(); // clear advertising data
Bluefruit.ScanResponse.clearData(); // clear scan response data
// Advertising packet
Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
Bluefruit.Advertising.addTxPower();
// Include the BLE UART (AKA 'NUS') 128-bit UUID
Bluefruit.Advertising.addService(bleuart);
// Secondary Scan Response packet (optional)
// Since there is no room for 'Name' in Advertising packet
Bluefruit.ScanResponse.addName();
/* Start Advertising
* - Enable auto advertising if disconnected
* - Interval: fast mode = 20 ms, slow mode = 152.5 ms
* - Timeout for fast mode is 30 seconds
* - Start(timeout) with timeout = 0 will advertise forever (until connected)
*
* For recommended advertising interval
* https://developer.apple.com/library/content/qa/qa1931/_index.html
*/
Bluefruit.Advertising.restartOnDisconnect(false); // don't restart automatically as we handle it in onDisconnect
Bluefruit.Advertising.setInterval(32, 244);
Bluefruit.Advertising.setFastTimeout(30); // number of seconds in fast mode
Bluefruit.Advertising.start(0); // 0 = Don't stop advertising after n seconds
Bluefruit.Advertising.setFastTimeout(30);
Bluefruit.Advertising.restartOnDisconnect(true);
}
void SerialBLEInterface::stopAdv() {
void SerialBLEInterface::clearBuffers() {
send_queue_len = 0;
recv_queue_len = 0;
bleuart.flush();
}
BLE_DEBUG_PRINTLN("SerialBLEInterface: stopping advertising");
// we only want to stop advertising if it's running, otherwise an invalid state error is logged by ble stack
if(!Bluefruit.Advertising.isRunning()){
return;
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];
}
}
// stop advertising
Bluefruit.Advertising.stop();
}
// ---------- public methods
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];
}
}
}
void SerialBLEInterface::enable() {
bool SerialBLEInterface::isValidConnection(uint16_t handle, bool requireWaitingForSecurity) const {
if (_conn_handle != handle) {
return false;
}
BLEConnection* conn = Bluefruit.Connection(handle);
if (conn == nullptr || !conn->connected()) {
return false;
}
if (requireWaitingForSecurity && _isDeviceConnected) {
return false;
}
return true;
}
bool SerialBLEInterface::isAdvertising() const {
ble_gap_addr_t adv_addr;
uint32_t err_code = sd_ble_gap_adv_addr_get(0, &adv_addr);
return (err_code == NRF_SUCCESS);
}
void SerialBLEInterface::enable() {
if (_isEnabled) return;
_isEnabled = true;
clearBuffers();
_last_health_check = millis();
// Start advertising
startAdv();
Bluefruit.Advertising.start(0);
}
void SerialBLEInterface::disconnect() {
if (_conn_handle != BLE_CONN_HANDLE_INVALID) {
sd_ble_gap_disconnect(_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
}
}
void SerialBLEInterface::disable() {
_isEnabled = false;
BLE_DEBUG_PRINTLN("SerialBLEInterface::disable");
BLE_DEBUG_PRINTLN("SerialBLEInterface: disable");
#ifdef RAK_BOARD
Bluefruit.disconnect(Bluefruit.connHandle());
#else
uint16_t conn_id;
if (Bluefruit.getConnectedHandles(&conn_id, 1) > 0) {
Bluefruit.disconnect(conn_id);
}
#endif
Bluefruit.Advertising.restartOnDisconnect(false);
disconnect();
Bluefruit.Advertising.stop();
Bluefruit.Advertising.clearData();
stopAdv();
_last_health_check = 0;
}
size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) {
if (len > MAX_FRAME_SIZE) {
BLE_DEBUG_PRINTLN("writeFrame(), frame too big, len=%d", len);
BLE_DEBUG_PRINTLN("writeFrame(), frame too big, len=%u", (unsigned)len);
return 0;
}
if (_isDeviceConnected && len > 0) {
bool connected = isConnected();
if (connected && len > 0) {
if (send_queue_len >= FRAME_QUEUE_SIZE) {
BLE_DEBUG_PRINTLN("writeFrame(), send_queue is full!");
return 0;
}
send_queue[send_queue_len].len = len; // add to send queue
send_queue[send_queue_len].len = len;
memcpy(send_queue[send_queue_len].buf, src, len);
send_queue_len++;
return len;
}
return 0;
}
#define BLE_WRITE_MIN_INTERVAL 60
bool SerialBLEInterface::isWriteBusy() const {
return millis() < _last_write + BLE_WRITE_MIN_INTERVAL; // still too soon to start another write?
}
size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) {
if (send_queue_len > 0 // first, check send queue
&& millis() >= _last_write + BLE_WRITE_MIN_INTERVAL // space the writes apart
) {
_last_write = millis();
bleuart.write(send_queue[0].buf, send_queue[0].len);
BLE_DEBUG_PRINTLN("writeBytes: sz=%d, hdr=%d", (uint32_t)send_queue[0].len, (uint32_t) send_queue[0].buf[0]);
send_queue_len--;
for (int i = 0; i < send_queue_len; i++) { // delete top item from queue
send_queue[i] = send_queue[i + 1];
}
} else {
int len = bleuart.available();
if (len > 0) {
bleuart.readBytes(dest, len);
BLE_DEBUG_PRINTLN("readBytes: sz=%d, hdr=%d", len, (uint32_t) dest[0]);
return len;
if (send_queue_len > 0) {
if (!isConnected()) {
BLE_DEBUG_PRINTLN("writeBytes: connection invalid, clearing send queue");
send_queue_len = 0;
} else {
Frame frame_to_send = send_queue[0];
size_t written = bleuart.write(frame_to_send.buf, frame_to_send.len);
if (written == frame_to_send.len) {
BLE_DEBUG_PRINTLN("writeBytes: sz=%u, hdr=%u", (unsigned)frame_to_send.len, (unsigned)frame_to_send.buf[0]);
shiftSendQueueLeft();
} else if (written > 0) {
BLE_DEBUG_PRINTLN("writeBytes: partial write, sent=%u of %u, dropping corrupted frame", (unsigned)written, (unsigned)frame_to_send.len);
shiftSendQueueLeft();
} else {
if (!isConnected()) {
BLE_DEBUG_PRINTLN("writeBytes failed: connection lost, dropping frame");
shiftSendQueueLeft();
} else {
BLE_DEBUG_PRINTLN("writeBytes failed (buffer full), keeping frame for retry");
}
}
}
}
if (recv_queue_len > 0) {
size_t len = recv_queue[0].len;
memcpy(dest, recv_queue[0].buf, len);
BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, (unsigned)dest[0]);
shiftRecvQueueLeft();
return len;
}
// Advertising watchdog: periodically check if advertising is running, restart if not
// Only run when truly disconnected (no connection handle), not during connection establishment
unsigned long now = millis();
if (_isEnabled && !isConnected() && _conn_handle == BLE_CONN_HANDLE_INVALID) {
if (now - _last_health_check >= BLE_HEALTH_CHECK_INTERVAL) {
_last_health_check = now;
if (!isAdvertising()) {
BLE_DEBUG_PRINTLN("SerialBLEInterface: advertising watchdog - advertising stopped, restarting");
Bluefruit.Advertising.start(0);
}
}
}
return 0;
}
bool SerialBLEInterface::isConnected() const {
return _isDeviceConnected;
void SerialBLEInterface::onBleUartRX(uint16_t conn_handle) {
if (!instance) {
return;
}
if (instance->_conn_handle != conn_handle || !instance->isConnected()) {
while (instance->bleuart.available() > 0) {
instance->bleuart.read();
}
return;
}
while (instance->bleuart.available() > 0) {
if (instance->recv_queue_len >= FRAME_QUEUE_SIZE) {
while (instance->bleuart.available() > 0) {
instance->bleuart.read();
}
BLE_DEBUG_PRINTLN("onBleUartRX: recv queue full, dropping data");
break;
}
int avail = instance->bleuart.available();
if (avail > MAX_FRAME_SIZE) {
BLE_DEBUG_PRINTLN("onBleUartRX: WARN: BLE RX overflow, avail=%d, draining all", avail);
uint8_t drain_buf[32];
while (instance->bleuart.available() > 0) {
int chunk = instance->bleuart.available() > 32 ? 32 : instance->bleuart.available();
instance->bleuart.readBytes(drain_buf, chunk);
}
continue;
}
int read_len = avail;
instance->recv_queue[instance->recv_queue_len].len = read_len;
instance->bleuart.readBytes(instance->recv_queue[instance->recv_queue_len].buf, read_len);
instance->recv_queue_len++;
}
}
bool SerialBLEInterface::isConnected() const {
return _isDeviceConnected && Bluefruit.connected() > 0;
}
bool SerialBLEInterface::isWriteBusy() const {
return send_queue_len >= (FRAME_QUEUE_SIZE - 1);
}

View file

@ -11,41 +11,51 @@ class SerialBLEInterface : public BaseSerialInterface {
BLEUart bleuart;
bool _isEnabled;
bool _isDeviceConnected;
unsigned long _last_write;
uint16_t _conn_handle;
unsigned long _last_health_check;
struct Frame {
uint8_t len;
uint8_t buf[MAX_FRAME_SIZE];
};
#define FRAME_QUEUE_SIZE 4
int send_queue_len;
#define FRAME_QUEUE_SIZE 12
uint8_t send_queue_len;
Frame send_queue[FRAME_QUEUE_SIZE];
uint8_t recv_queue_len;
Frame recv_queue[FRAME_QUEUE_SIZE];
void clearBuffers() { send_queue_len = 0; }
void clearBuffers();
void shiftSendQueueLeft();
void shiftRecvQueueLeft();
bool isValidConnection(uint16_t handle, bool requireWaitingForSecurity = false) const;
bool isAdvertising() const;
static void onConnect(uint16_t connection_handle);
static void onDisconnect(uint16_t connection_handle, uint8_t reason);
static void onSecured(uint16_t connection_handle);
static bool onPairingPasskey(uint16_t connection_handle, uint8_t const passkey[6], bool match_request);
static void onPairingComplete(uint16_t connection_handle, uint8_t auth_status);
static void onBLEEvent(ble_evt_t* evt);
static void onBleUartRX(uint16_t conn_handle);
public:
SerialBLEInterface() {
_isEnabled = false;
_isDeviceConnected = false;
_last_write = 0;
_conn_handle = BLE_CONN_HANDLE_INVALID;
_last_health_check = 0;
send_queue_len = 0;
recv_queue_len = 0;
}
void startAdv();
void stopAdv();
void begin(const char* device_name, uint32_t pin_code);
// BaseSerialInterface methods
void disconnect();
void enable() override;
void disable() override;
bool isEnabled() const override { return _isEnabled; }
bool isConnected() const override;
bool isWriteBusy() const override;
size_t writeFrame(const uint8_t src[], size_t len) override;
size_t checkRecvFrame(uint8_t dest[]) override;