hi guys
I have been struggling to set up a stable connection between my sensors. No matter what I do, If I move my sensors more than 50cm away from each other, the BLE connection becomes unstable, devices drops connections.
Does anyone have a good working examples when 1 sensor is in dual role and 1 or more in a peripherial role?
the current code has tx.power 4, but 8 is the exactly the same
(post deleted by author)
(post deleted by author)
/*
* ============================================================================
* XIAO nRF52840 Sense — Boxing Glove BLE Sensor Firmware
* ============================================================================
*
* Board: Seeed XIAO nRF52840 Sense
* IMU: Built-in LSM6DS3 (I2C)
* BLE stack: Adafruit Bluefruit (nRF52 core)
* Role: BLE peripheral streaming IMU data to a central "hub"
*
* This firmware is intentionally simple:
* - No ring buffers, no batching, no punch detection, no filtering.
* - One CSV line per sample, sent only while a central is connected.
* - If disconnected, sampling/sending stops and advertising restarts.
* - Nothing is queued up while disconnected.
*
* To build the LEFT glove vs the RIGHT glove, change GLOVE_ID below and
* reflash. Nothing else needs to change between the two builds.
* ============================================================================
*/
#include <bluefruit.h>
#include <Wire.h>
#include "LSM6DS3.h"
#include <nrf.h>
// ============================================================================
// GLOVE IDENTITY — change this one line per build
// ============================================================================
#define GLOVE_ID 'L' // 'L' for left glove, 'R' for right glove
// ============================================================================
// SAMPLING CONFIG
// ============================================================================
const uint32_t SAMPLE_INTERVAL_MS = 100; // 100 ms for now (testing)
#define PACKET_VERSION 1
#if GLOVE_ID == 'L'
#define SENSOR_ID 1
#elif GLOVE_ID == 'R'
#define SENSOR_ID 2
#else
#error "GLOVE_ID must be 'L' or 'R'"
#endif
struct __attribute__((packed)) ImuPacket
{
uint8_t version;
uint8_t sensorId;
uint8_t sequence;
uint8_t reserved;
uint32_t timestamp;
int16_t ax;
int16_t ay;
int16_t az;
int16_t gx;
int16_t gy;
int16_t gz;
};
static_assert(sizeof(ImuPacket) == 20, "ImuPacket must be 20 bytes");
// ============================================================================
// BLE OBJECTS
// ============================================================================
BLEUart bleuart;
BLEDis bledis;
// ============================================================================
// IMU
// ============================================================================
LSM6DS3 imu(I2C_MODE, 0x6A);
bool imuOk = false;
// ============================================================================
// BATTERY (XIAO nRF52840 Sense known pins)
// ============================================================================
// PIN_VBAT / VBAT_ENABLE are provided by the Seeed board variant files.
// VBAT_ENABLE is held LOW permanently (per Seeed guidance — do not toggle
// it per-reading) to connect the onboard voltage divider to PIN_VBAT.
#if defined(PIN_VBAT) && defined(VBAT_ENABLE)
#define VBAT_SUPPORTED 1
static const float VBAT_ADC_REFERENCE_MV = 3600.0f; // internal ref, mV
static const float VBAT_ADC_MAX_COUNT = 4096.0f; // 12-bit
static const float VBAT_DIVIDER_RATIO = 1510.0f / 510.0f; // (1M+510k)/510k
#else
#define VBAT_SUPPORTED 0
#endif
// ============================================================================
// STATE
// ============================================================================
volatile bool bleConnected = false;
uint32_t lastSampleMs = 0;
uint32_t lastDiagMs = 0;
// Diagnostic counters (reset each 1-second window where noted)
uint32_t totalPacketsSent = 0;
uint32_t packetsSentThisSec = 0;
uint32_t imuReadsThisSec = 0;
uint32_t totalPacketsSkipped = 0;
uint8_t packetSequence = 0;
// LED error blink state
bool imuErrorLedState = false;
uint32_t lastImuErrorBlinkMs = 0;
// Connection handle (needed so we can gracefully disconnect before sleep)
uint16_t connHandle = BLE_CONN_HANDLE_INVALID;
// ============================================================================
// DEEP SLEEP (wake on motion via IMU INT1)
// ============================================================================
const uint32_t DEEP_SLEEP_TIMEOUT_MS = 5UL * 60UL * 1000UL; // 5 minutes
const float MOTION_THRESHOLD_G = 0.05f; // tune this
uint32_t lastMotionMs = 0;
float refAx = 0, refAy = 0, refAz = 0;
bool refInitialized = false;
// ============================================================================
// FORWARD DECLARATIONS
// ============================================================================
void startAdvertising();
void connect_callback(uint16_t conn_handle);
void disconnect_callback(uint16_t conn_handle, uint8_t reason);
void printBootBanner();
void printResetReason();
void setLed(bool r, bool g, bool b);
void updateImuErrorLed();
void printDiagnostics();
void enterDeepSleep();
// ============================================================================
// SETUP
// ============================================================================
void setup()
{
Serial.begin(115200);
uint32_t serialWaitStart = millis();
while (!Serial && (millis() - serialWaitStart) < 3000)
{
delay(10);
}
// ---- Boot banner ----
printBootBanner();
// ---- Reset reason (read, print, then clear) ----
printResetReason();
// ---- LEDs ----
pinMode(LED_RED, OUTPUT);
pinMode(LED_GREEN, OUTPUT);
pinMode(LED_BLUE, OUTPUT);
setLed(false, false, false); // off (LEDs are active LOW on this board)
// ---- Battery ADC setup ----
#if VBAT_SUPPORTED
pinMode(VBAT_ENABLE, OUTPUT);
digitalWrite(VBAT_ENABLE, LOW);
pinMode(PIN_VBAT, INPUT);
analogReadResolution(12);
#endif
// ---- IMU init ----
Wire.begin();
if (imu.begin() != 0)
{
Serial.println("IMU INIT FAILED");
imuOk = false;
}
else
{
Serial.println("IMU INIT OK");
imuOk = true;
}
// ---- BLE init ----
Bluefruit.begin();
Bluefruit.setTxPower(4);
char bleName[24];
snprintf(bleName, sizeof(bleName), "XIAO-Glove-%c", (char)GLOVE_ID);
Bluefruit.setName(bleName);
Bluefruit.Periph.setConnectCallback(connect_callback);
Bluefruit.Periph.setDisconnectCallback(disconnect_callback);
bledis.setManufacturer("Seeed Studio");
bledis.setModel("XIAO nRF52840 Sense Boxing Glove");
bledis.begin();
bleuart.begin();
startAdvertising();
lastSampleMs = millis();
lastDiagMs = millis();
lastMotionMs = millis();
}
// ============================================================================
// LOOP
// ============================================================================
void loop()
{
uint32_t now = millis();
// ---- IMU error LED (blinks continuously if IMU failed to init) ----
updateImuErrorLed();
// ---- Sample + send, only while connected, at the configured interval ----
if ((now - lastSampleMs) >= SAMPLE_INTERVAL_MS)
{
lastSampleMs = now;
if (bleConnected && imuOk)
{
float ax = imu.readFloatAccelX();
float ay = imu.readFloatAccelY();
float az = imu.readFloatAccelZ();
float gx = imu.readFloatGyroX();
float gy = imu.readFloatGyroY();
float gz = imu.readFloatGyroZ();
imuReadsThisSec++;
// ---- Motion tracking for deep sleep timeout ----
if (!refInitialized)
{
refAx = ax; refAy = ay; refAz = az;
refInitialized = true;
lastMotionMs = now;
}
else
{
float delta = fabs(ax - refAx) + fabs(ay - refAy) + fabs(az - refAz);
if (delta > MOTION_THRESHOLD_G)
{
lastMotionMs = now;
refAx = ax; refAy = ay; refAz = az;
}
}
if (bleuart.notifyEnabled())
{
ImuPacket pkt;
pkt.version = PACKET_VERSION;
pkt.sensorId = SENSOR_ID;
pkt.sequence = packetSequence++;
pkt.reserved = 0;
pkt.timestamp = now;
// accel: g -> milli-g
pkt.ax = (int16_t)(ax * 1000.0f);
pkt.ay = (int16_t)(ay * 1000.0f);
pkt.az = (int16_t)(az * 1000.0f);
// gyro: deg/sec -> 0.1 deg/sec
pkt.gx = (int16_t)(gx * 10.0f);
pkt.gy = (int16_t)(gy * 10.0f);
pkt.gz = (int16_t)(gz * 10.0f);
bleuart.write((uint8_t*)&pkt, sizeof(pkt));
totalPacketsSent++;
packetsSentThisSec++;
}
else
{
// Connected at the link layer but the central hasn't enabled
// notifications yet — don't build up any backlog, just skip.
totalPacketsSkipped++;
}
}
else if (!bleConnected)
{
// Not connected: do nothing. No buffering, no queueing.
totalPacketsSkipped++;
}
}
// ---- Deep sleep timeout ----
if (refInitialized && (now - lastMotionMs) >= DEEP_SLEEP_TIMEOUT_MS)
{
enterDeepSleep();
}
// ---- Diagnostics, once per second ----
if ((now - lastDiagMs) >= 1000)
{
lastDiagMs = now;
printDiagnostics();
packetsSentThisSec = 0;
imuReadsThisSec = 0;
}
}
// ============================================================================
// BLE ADVERTISING
// ============================================================================
void startAdvertising()
{
Bluefruit.Advertising.clearData();
Bluefruit.ScanResponse.clearData();
Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
Bluefruit.Advertising.addTxPower();
Bluefruit.Advertising.addService(bleuart);
Bluefruit.ScanResponse.addName();
Bluefruit.Advertising.restartOnDisconnect(true);
Bluefruit.Advertising.setInterval(32, 244); // 20ms - 152.5ms
Bluefruit.Advertising.setFastTimeout(30);
Bluefruit.Advertising.start(0); // 0 = advertise forever until connected
Serial.println("ADVERTISING STARTED");
if (imuOk)
{
setLed(false, false, true); // blue = advertising
}
}
// ============================================================================
// BLE CALLBACKS (kept short — no blocking work here)
// ============================================================================
void connect_callback(uint16_t conn_handle)
{
connHandle = conn_handle;
bleConnected = true;
Serial.println("BLE CONNECTED");
setLed(false, true, false); // green = connected
}
void disconnect_callback(uint16_t conn_handle, uint8_t reason)
{
(void) conn_handle;
bleConnected = false;
connHandle = BLE_CONN_HANDLE_INVALID;
Serial.print("BLE DISCONNECTED reason=0x");
Serial.println(reason, HEX);
// Bluefruit's restartOnDisconnect(true) handles re-advertising for us.
// We just reflect the LED state; the "ADVERTISING STARTED" print for
// the automatic restart is not re-emitted here since Bluefruit does not
// expose a callback for it — the LED still switches back to blue.
if (imuOk)
{
setLed(false, false, true); // blue = advertising again
}
}
// ============================================================================
// BOOT BANNER
// ============================================================================
void printBootBanner()
{
char nameBuf[24];
snprintf(nameBuf, sizeof(nameBuf), "XIAO-Glove-%c", (char)GLOVE_ID);
Serial.println("========================");
Serial.println("GLOVE BOOT");
Serial.print("Name: ");
Serial.println(nameBuf);
Serial.print("Sample interval: ");
Serial.print(SAMPLE_INTERVAL_MS);
Serial.println(" ms");
Serial.print("Build date: ");
Serial.println(__DATE__);
Serial.print("Build time: ");
Serial.println(__TIME__);
Serial.println("========================");
}
// ============================================================================
// RESET REASON (nRF52840 RESETREAS) — read, print, then clear
// ============================================================================
void printResetReason()
{
uint32_t resetReas = NRF_POWER->RESETREAS;
Serial.print("Reset reason: ");
if (resetReas == 0)
{
Serial.print("POWERON ");
}
if (resetReas & POWER_RESETREAS_RESETPIN_Msk) Serial.print("RESETPIN ");
if (resetReas & POWER_RESETREAS_DOG_Msk) Serial.print("WATCHDOG ");
if (resetReas & POWER_RESETREAS_LOCKUP_Msk) Serial.print("LOCKUP ");
if (resetReas & POWER_RESETREAS_SREQ_Msk) Serial.print("SREQ ");
if (resetReas & POWER_RESETREAS_OFF_Msk) Serial.print("OFF ");
if (resetReas & POWER_RESETREAS_LPCOMP_Msk) Serial.print("LPCOMP ");
if (resetReas & POWER_RESETREAS_DIF_Msk) Serial.print("DIF ");
if (resetReas & POWER_RESETREAS_NFC_Msk) Serial.print("NFC ");
if (resetReas & POWER_RESETREAS_VBUS_Msk) Serial.print("VBUS ");
Serial.println();
// Nordic-recommended clear: write back exactly the bits that were read,
// which clears only those bits and leaves the rest untouched.
NRF_POWER->RESETREAS = resetReas;
}
// ============================================================================
// DEEP SLEEP — enter after DEEP_SLEEP_TIMEOUT_MS with no significant
// motion on the IMU. Disconnects cleanly (if connected), arms the IMU's
// own wake-up-on-motion interrupt on INT1, then powers the nRF52840 off
// via the SoftDevice-safe call. Waking is a full reboot (setup() runs
// again) — advertising/reconnection resumes exactly as on a cold boot.
// ============================================================================
void enterDeepSleep()
{
Serial.println("No motion for 5 min — entering deep sleep");
Serial.flush();
// Stop advertising and disconnect cleanly if a central is attached
Bluefruit.Advertising.stop();
if (bleConnected && connHandle != BLE_CONN_HANDLE_INVALID)
{
Bluefruit.disconnect(connHandle);
}
delay(100);
// Configure IMU wake-up-on-motion, routed to INT1
imu.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_DUR, 0x00); // no extra duration
imu.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x02); // sensitivity, tune 1-63
imu.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x80); // enable interrupts, no latch
imu.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x20); // wake-up event -> INT1
imu.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x10); // low-power accel, 12.5Hz
delay(50); // let settings take effect before power-off
// Arm INT1 (P0.11) as the System OFF wake source
nrf_gpio_cfg_sense_input(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1),
NRF_GPIO_PIN_PULLDOWN,
NRF_GPIO_PIN_SENSE_HIGH);
// SoftDevice-safe power off (NOT NRF_POWER->SYSTEMOFF directly — SD owns POWER)
sd_power_system_off();
}
// ============================================================================
// FREE MEMORY (this core's FreeRTOS port wraps malloc()/free() from newlib
// instead of compiling in heap_4.c, so xPortGetFreeHeapSize() does not
// exist to link against here. This is the standard fallback: the gap
// between the current stack pointer and the current top of the malloc
// heap (sbrk(0)) is real, measured free space.)
// ============================================================================
extern "C" char *sbrk(int i);
static uint32_t freeMemoryBytes()
{
char stackTop;
return (uint32_t)(&stackTop - reinterpret_cast<char*>(sbrk(0)));
}
// ============================================================================
// LED HELPER
// ============================================================================
// XIAO nRF52840 (Sense) onboard RGB LED is active LOW.
void setLed(bool r, bool g, bool b)
{
digitalWrite(LED_RED, r ? LOW : HIGH);
digitalWrite(LED_GREEN, g ? LOW : HIGH);
digitalWrite(LED_BLUE, b ? LOW : HIGH);
}
void updateImuErrorLed()
{
if (imuOk)
{
return;
}
uint32_t now = millis();
if ((now - lastImuErrorBlinkMs) >= 300)
{
lastImuErrorBlinkMs = now;
imuErrorLedState = !imuErrorLedState;
setLed(imuErrorLedState, false, false); // red blink = IMU error
}
}
// ============================================================================
// DIAGNOSTICS — printed once per second
// ============================================================================
void printDiagnostics()
{
Serial.println("---- DIAG ----");
Serial.print("GLOVE_ID: ");
Serial.println((char)GLOVE_ID);
Serial.print("UPTIME_MS: ");
Serial.println(millis());
Serial.print("BLE_CONNECTED: ");
Serial.println(bleConnected ? "yes" : "no");
Serial.print("PACKETS_SENT_TOTAL: ");
Serial.println(totalPacketsSent);
Serial.print("PACKETS_SENT_PER_SEC: ");
Serial.println(packetsSentThisSec);
Serial.print("IMU_READS_PER_SEC: ");
Serial.println(imuReadsThisSec);
Serial.print("PACKETS_SKIPPED_DISCONNECTED: ");
Serial.println(totalPacketsSkipped);
Serial.print("FREE_HEAP: ");
Serial.println(freeMemoryBytes());
#if VBAT_SUPPORTED
{
uint32_t adcCount = analogRead(PIN_VBAT);
float adcMillivolts = (adcCount / VBAT_ADC_MAX_COUNT) * VBAT_ADC_REFERENCE_MV;
float batteryVolts = (adcMillivolts * VBAT_DIVIDER_RATIO) / 1000.0f;
Serial.print("VBAT: ");
Serial.print(batteryVolts, 2);
Serial.println("V");
}
#else
Serial.println("VBAT: not implemented");
#endif
Serial.println("--------------");
}
/*
* ============================================================================
* XIAO nRF52840 Sense — Boxing Hub Firmware
* ============================================================================
*
* Board: Seeed XIAO nRF52840 Sense
* BLE stack: Adafruit Bluefruit (nRF52 core)
*
* Role: this board is BOTH a BLE central (connects to two glove
* peripherals) AND a BLE peripheral (exposes one BLE UART service to a
* phone). The phone only ever talks to the hub — it never sees the
* gloves directly.
*
* Design goals for this firmware: reliability and simplicity over
* performance. No buffering, no batching, no packet merging, no
* reconnection cleverness beyond "reconnect and start over". If
* something goes wrong with one glove, only that glove is affected.
* ============================================================================
*/
#include <bluefruit.h>
#include <Wire.h>
#include "LSM6DS3.h"
#include <nrf.h>
// ============================================================================
// IDENTITY / NAMES
// ============================================================================
static const char* HUB_NAME = "BoxingGlove-Hub";
static const char* LEFT_NAME = "XIAO-Glove-L";
static const char* RIGHT_NAME = "XIAO-Glove-R";
struct __attribute__((packed)) ImuPacket
{
uint8_t version;
uint8_t sensorId;
uint8_t sequence;
uint8_t reserved;
uint32_t timestamp;
int16_t ax;
int16_t ay;
int16_t az;
int16_t gx;
int16_t gy;
int16_t gz;
};
static_assert(sizeof(ImuPacket) == 20, "ImuPacket must be 20 bytes");
// ============================================================================
// BAG IMU (the hub's own onboard sensor — third stream to the phone,
// tagged 'H', same rules as the gloves: sent only if phone is ready,
// never buffered/queued if not)
// ============================================================================
LSM6DS3 bagImu(I2C_MODE, 0x6A);
bool bagImuOk = false;
const uint32_t BAG_SAMPLE_INTERVAL_MS = 100;
uint32_t lastBagSampleMs = 0;
uint32_t bagImuReadsThisSec = 0;
// The hub streams its own IMU to the phone as a third sensor, using the
// same CSV line shape the gloves use, tagged with its own ID so the
// phone-side code can tell it apart from LEFT ('L') and RIGHT ('R').
const char BAG_SENSOR_ID = 'H';
uint32_t bagPacketsSentTotal = 0;
uint32_t bagPacketsSentThisSec = 0;
uint32_t bagPacketsSkippedThisSec = 0;
// ============================================================================
// BLE OBJECTS
// ============================================================================
// Peripheral side: the one service the phone connects to.
BLEUart bleuart;
// Central side: one independent client UART per glove.
BLEClientUart leftClient;
BLEClientUart rightClient;
// Max bytes we'll ever pull out of a glove's UART in one read. The glove
// sends a single short CSV line per sample, so this is generous headroom.
static const size_t GLOVE_PACKET_BUF_SIZE = 128;
// ============================================================================
// PER-GLOVE STATE
// ============================================================================
// Each glove gets its own independent block of state. Nothing here is
// shared between LEFT and RIGHT except the struct definition itself —
// every decision is made against one specific instance.
struct GloveLink
{
const char* name;
BLEClientUart* client;
uint16_t connHandle;
bool connecting;
bool connected;
bool ready;
ble_gap_addr_t pendingAddr;
bool hasPendingAddr;
uint32_t packetNumber;
uint32_t packetsReceivedTotal;
uint32_t packetsThisSec;
bool rssiMonitoring;
uint8_t rxBuffer[256];
size_t rxLength;
};
GloveLink leftLink = { LEFT_NAME, &leftClient, BLE_CONN_HANDLE_INVALID, false, false, false, {}, false, 0, 0, 0, false };
GloveLink rightLink = { RIGHT_NAME, &rightClient, BLE_CONN_HANDLE_INVALID, false, false, false, {}, false, 0, 0, 0, false };
// ============================================================================
// PHONE STATE
// ============================================================================
bool phoneConnected = false;
bool phoneReady = false; // true once the phone has enabled notifications
uint16_t phoneConnHandle = BLE_CONN_HANDLE_INVALID;
// ============================================================================
// LED (RGB, active driven via analogWrite so we can mix real colors —
// 0 = fully off, 255 = fully on, board is active-LOW so we invert)
// ============================================================================
uint32_t discoveryFailureBlinkUntilMs = 0; // orange blink runs until this time
// ============================================================================
// DIAGNOSTICS TIMER
// ============================================================================
uint32_t lastDiagMs = 0;
// ============================================================================
// DEEP SLEEP (wake on motion via bag IMU INT1)
// ============================================================================
const uint32_t DEEP_SLEEP_TIMEOUT_MS = 5UL * 60UL * 1000UL; // 5 minutes
const float MOTION_THRESHOLD_G = 0.05f; // tune this
uint32_t lastMotionMs = 0;
float refBagAx = 0, refBagAy = 0, refBagAz = 0;
bool refBagInitialized = false;
// ============================================================================
// FORWARD DECLARATIONS
// ============================================================================
void printBootBanner();
void printResetReason();
void enterDeepSleep();
void startScanIfNeeded();
void scan_callback(ble_gap_evt_adv_report_t* report);
void cent_connect_callback(uint16_t conn_handle);
void cent_disconnect_callback(uint16_t conn_handle, uint8_t reason);
void leftClient_rx_callback(BLEClientUart& uart);
void rightClient_rx_callback(BLEClientUart& uart);
void handleGlovePacket(GloveLink& link, BLEClientUart& uart);
void prph_connect_callback(uint16_t conn_handle);
void prph_disconnect_callback(uint16_t conn_handle, uint8_t reason);
bool extractAdvName(const ble_gap_evt_adv_report_t* report, char* outName, size_t outSize);
bool addrEquals(const ble_gap_addr_t& a, const ble_gap_addr_t& b);
void setLedColor(uint8_t r, uint8_t g, uint8_t b);
void updateLed();
void triggerDiscoveryFailureBlink();
void printDiagnostics();
// ============================================================================
// SETUP
// ============================================================================
void setup()
{
Serial.begin(115200);
uint32_t serialWaitStart = millis();
while (!Serial && (millis() - serialWaitStart) < 3000)
{
delay(10);
}
printBootBanner();
printResetReason();
// ---- LED ----
pinMode(LED_RED, OUTPUT);
pinMode(LED_GREEN, OUTPUT);
pinMode(LED_BLUE, OUTPUT);
setLedColor(0, 0, 0);
// ---- Bag IMU (hub's own onboard sensor) ----
Wire.begin();
if (bagImu.begin() != 0)
{
Serial.println("BAG IMU INIT FAILED");
bagImuOk = false;
}
else
{
Serial.println("BAG IMU INIT OK");
bagImuOk = true;
}
// ---- BLE bring-up ----
// 1 peripheral connection (the phone) + 2 central connections (the gloves).
Bluefruit.configPrphBandwidth(BANDWIDTH_MAX);
Bluefruit.configCentralBandwidth(BANDWIDTH_MAX);
Bluefruit.begin(1, 2);
Bluefruit.setTxPower(4);
Bluefruit.setName(HUB_NAME);
// Peripheral (phone-facing) callbacks
Bluefruit.Periph.setConnectCallback(prph_connect_callback);
Bluefruit.Periph.setDisconnectCallback(prph_disconnect_callback);
// Central (glove-facing) callbacks
Bluefruit.Central.setConnectCallback(cent_connect_callback);
Bluefruit.Central.setDisconnectCallback(cent_disconnect_callback);
// Phone-facing BLE UART service
bleuart.begin();
// Glove-facing BLE UART clients — completely independent objects,
// each with its own callback function (not a shared one).
leftClient.begin();
leftClient.setRxCallback(leftClient_rx_callback);
rightClient.begin();
rightClient.setRxCallback(rightClient_rx_callback);
// ---- Advertise to the phone ----
Bluefruit.Advertising.clearData();
Bluefruit.ScanResponse.clearData();
Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
Bluefruit.Advertising.addTxPower();
Bluefruit.Advertising.addService(bleuart);
Bluefruit.ScanResponse.addName();
Bluefruit.Advertising.restartOnDisconnect(true);
Bluefruit.Advertising.setInterval(32, 244); // 20ms - 152.5ms
Bluefruit.Advertising.setFastTimeout(30);
Bluefruit.Advertising.start(0);
Serial.println("ADVERTISING STARTED (phone)");
// ---- Scan for gloves ----
// Active scanning is required so we actually receive the scan-response
// packet that carries each glove's advertised name.
Bluefruit.Scanner.setRxCallback(scan_callback);
Bluefruit.Scanner.restartOnDisconnect(true);
Bluefruit.Scanner.useActiveScan(true);
Bluefruit.Scanner.setInterval(160, 80);
Bluefruit.Scanner.start(0);
Serial.println("SCANNING STARTED (gloves)");
lastDiagMs = millis();
lastBagSampleMs = millis();
lastMotionMs = millis();
}
// ============================================================================
// LOOP
// ============================================================================
void loop()
{
uint32_t now = millis();
// ---- Bag IMU: read once per interval, forward to phone as a third
// sensor stream (same CSV shape as the gloves, tagged 'H'). Only sent
// while the phone is ready; never buffered if it isn't. ----
if (bagImuOk && (now - lastBagSampleMs) >= BAG_SAMPLE_INTERVAL_MS)
{
lastBagSampleMs = now;
float ax = bagImu.readFloatAccelX();
float ay = bagImu.readFloatAccelY();
float az = bagImu.readFloatAccelZ();
float gx = bagImu.readFloatGyroX();
float gy = bagImu.readFloatGyroY();
float gz = bagImu.readFloatGyroZ();
bagImuReadsThisSec++;
// ---- Motion tracking for deep sleep timeout ----
if (!refBagInitialized)
{
refBagAx = ax; refBagAy = ay; refBagAz = az;
refBagInitialized = true;
lastMotionMs = now;
}
else
{
float delta = fabs(ax - refBagAx) + fabs(ay - refBagAy) + fabs(az - refBagAz);
if (delta > MOTION_THRESHOLD_G)
{
lastMotionMs = now;
refBagAx = ax; refBagAy = ay; refBagAz = az;
}
}
char line[96];
int len = snprintf(line, sizeof(line),
"%c,%lu,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
BAG_SENSOR_ID,
(unsigned long)now,
ax, ay, az,
gx, gy, gz);
if (len > 0)
{
Serial.print("HUB: ");
Serial.print(line);
if (phoneReady)
{
bleuart.write((uint8_t*)line, (size_t)len);
bagPacketsSentTotal++;
bagPacketsSentThisSec++;
}
else
{
bagPacketsSkippedThisSec++;
}
}
else
{
// Phone not ready: drop this sample, don't queue it for later.
bagPacketsSkippedThisSec++;
}
}
// ---- Phone notify-enabled edge detection ----
// BLEUart doesn't need a CCCD callback for this — a cheap poll here is
// enough, and keeps this out of any BLE callback entirely.
bool notifyNow = bleuart.notifyEnabled();
if (notifyNow && !phoneReady)
{
phoneReady = true;
Serial.println("PHONE READY");
}
else if (!notifyNow && phoneReady)
{
phoneReady = false;
}
// ---- LED ----
updateLed();
// ---- Deep sleep timeout ----
if (refBagInitialized && (now - lastMotionMs) >= DEEP_SLEEP_TIMEOUT_MS)
{
enterDeepSleep();
}
// ---- Diagnostics, once per second ----
if ((now - lastDiagMs) >= 1000)
{
lastDiagMs = now;
printDiagnostics();
leftLink.packetsThisSec = 0;
rightLink.packetsThisSec = 0;
bagImuReadsThisSec = 0;
bagPacketsSentThisSec = 0;
bagPacketsSkippedThisSec = 0;
}
}
// ============================================================================
// SCANNER
// ============================================================================
// Fires for every advertisement AND every scan-response packet received
// (active scanning is on). The glove's name lives in its scan-response
// packet, so this callback may see a given physical glove twice — once
// with no name, once with the name. Only the one carrying a name that
// matches LEFT_NAME/RIGHT_NAME is ever acted on. Everything else is
// ignored and scanning resumes.
void scan_callback(ble_gap_evt_adv_report_t* report)
{
char advName[32] = { 0 };
bool gotName = extractAdvName(report, advName, sizeof(advName));
if (!gotName)
{
Bluefruit.Scanner.resume();
return;
}
if (strcmp(advName, LEFT_NAME) == 0)
{
if (!leftLink.ready && !leftLink.connecting)
{
Serial.println("SCANNER: found LEFT, connecting");
leftLink.connecting = true;
leftLink.pendingAddr = report->peer_addr;
leftLink.hasPendingAddr = true;
Bluefruit.Central.connect(report);
return;
}
}
else if (strcmp(advName, RIGHT_NAME) == 0)
{
if (!rightLink.ready && !rightLink.connecting)
{
Serial.println("SCANNER: found RIGHT, connecting");
rightLink.connecting = true;
rightLink.pendingAddr = report->peer_addr;
rightLink.hasPendingAddr = true;
Bluefruit.Central.connect(report);
return;
}
}
// Not one of our gloves, or that role's slot is already taken/connecting.
Bluefruit.Scanner.resume();
}
void startScanIfNeeded()
{
if (leftLink.ready && rightLink.ready)
{
return; // both slots full, nothing to look for
}
if (!Bluefruit.Scanner.isRunning())
{
Bluefruit.Scanner.start(0);
}
}
// ============================================================================
// LEFT / RIGHT CONNECTION MANAGEMENT
// ============================================================================
// One shared entry point (Bluefruit only allows one central connect
// callback), but the FIRST thing it does is figure out — by address,
// recorded at scan time, never by getPeerName() — which independent
// GloveLink this event belongs to. Everything after that point only
// touches that one struct.
void cent_connect_callback(uint16_t conn_handle)
{
BLEConnection* connection = Bluefruit.Connection(conn_handle);
ble_gap_addr_t peerAddr = connection->getPeerAddr();
GloveLink* link = nullptr;
if (leftLink.connecting && leftLink.hasPendingAddr && addrEquals(peerAddr, leftLink.pendingAddr))
{
link = &leftLink;
}
else if (rightLink.connecting && rightLink.hasPendingAddr && addrEquals(peerAddr, rightLink.pendingAddr))
{
link = &rightLink;
}
if (link == nullptr)
{
// Shouldn't happen — we only ever call connect() right after
// recording the pending address — but fail safe if it does.
Serial.println("HUB: connected peer did not match any pending glove, disconnecting");
Bluefruit.disconnect(conn_handle);
startScanIfNeeded();
return;
}
link->connHandle = conn_handle;
link->connected = true;
link->hasPendingAddr = false;
Serial.print(link->name);
Serial.println(" CONNECTED");
connection->monitorRssi();
link->rssiMonitoring = true;
// ---- Discover UART, then enable notifications ----
bool discoverOk = link->client->discover(conn_handle);
if (!discoverOk)
{
Serial.print(link->name);
Serial.println(" UART DISCOVERY FAILED — disconnecting this glove only");
link->connected = false;
link->connecting = false;
link->connHandle = BLE_CONN_HANDLE_INVALID;
link->rssiMonitoring = false;
triggerDiscoveryFailureBlink();
Bluefruit.disconnect(conn_handle);
startScanIfNeeded();
return;
}
link->client->enableTXD();
link->ready = true;
link->connecting = false;
Serial.print(link->name);
Serial.println(" READY");
// Keep scanning if the other glove still isn't ready.
startScanIfNeeded();
}
void cent_disconnect_callback(uint16_t conn_handle, uint8_t reason)
{
GloveLink* link = nullptr;
if (leftLink.connHandle == conn_handle)
{
link = &leftLink;
}
else if (rightLink.connHandle == conn_handle)
{
link = &rightLink;
}
if (link == nullptr)
{
startScanIfNeeded();
return;
}
Serial.print(link->name);
Serial.print(" DISCONNECTED reason=0x");
Serial.println(reason, HEX);
link->connected = false;
link->ready = false;
link->connecting = false;
link->connHandle = BLE_CONN_HANDLE_INVALID;
link->hasPendingAddr = false;
link->rssiMonitoring = false;
startScanIfNeeded();
}
// ============================================================================
// GLOVE PACKET RECEPTION — one independent callback per glove
// ============================================================================
void leftClient_rx_callback(BLEClientUart& uart)
{
handleGlovePacket(leftLink, uart);
}
void rightClient_rx_callback(BLEClientUart& uart)
{
handleGlovePacket(rightLink, uart);
}
// Not "shared guessing logic" — the caller has already told us exactly
// which glove this is. This just avoids writing the same ten lines twice.
void handleGlovePacket(GloveLink& link, BLEClientUart& uart)
{
uint8_t temp[64];
while (uart.available())
{
int len = uart.read(temp, sizeof(temp));
if (len <= 0)
return;
// Append new bytes into receive buffer
if (link.rxLength + len > sizeof(link.rxBuffer))
{
Serial.print(link.name);
Serial.println(": RX buffer overflow");
link.rxLength = 0;
return;
}
memcpy(link.rxBuffer + link.rxLength, temp, len);
link.rxLength += len;
// Process every complete packet
while (link.rxLength >= sizeof(ImuPacket))
{
ImuPacket pkt;
memcpy(&pkt, link.rxBuffer, sizeof(ImuPacket));
// Remove processed packet from buffer
memmove(link.rxBuffer,
link.rxBuffer + sizeof(ImuPacket),
link.rxLength - sizeof(ImuPacket));
link.rxLength -= sizeof(ImuPacket);
link.packetNumber++;
link.packetsReceivedTotal++;
link.packetsThisSec++;
// =====================================================
// HUB SERIAL MONITOR
// =====================================================
Serial.print(link.name);
Serial.print(": ");
Serial.print(pkt.sensorId == 1 ? 'L' : 'R');
Serial.print(",");
Serial.print(pkt.timestamp);
Serial.print(",");
Serial.print(pkt.ax / 1000.0f, 2);
Serial.print(",");
Serial.print(pkt.ay / 1000.0f, 2);
Serial.print(",");
Serial.print(pkt.az / 1000.0f, 2);
Serial.print(",");
Serial.print(pkt.gx / 10.0f, 2);
Serial.print(",");
Serial.print(pkt.gy / 10.0f, 2);
Serial.print(",");
Serial.println(pkt.gz / 10.0f, 2);
// =====================================================
// PHONE (CSV for now)
// =====================================================
if (phoneReady)
{
char line[96];
int outLen = snprintf(
line,
sizeof(line),
"%c,%lu,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
pkt.sensorId == 1 ? 'L' : 'R',
(unsigned long)pkt.timestamp,
pkt.ax / 1000.0f,
pkt.ay / 1000.0f,
pkt.az / 1000.0f,
pkt.gx / 10.0f,
pkt.gy / 10.0f,
pkt.gz / 10.0f);
if (outLen > 0)
{
bleuart.write((uint8_t*)line, (size_t)outLen);
}
}
}
}
}
// ============================================================================
// PHONE (peripheral) CALLBACKS — small, flags + a print only
// ============================================================================
void prph_connect_callback(uint16_t conn_handle)
{
phoneConnHandle = conn_handle;
phoneConnected = true;
phoneReady = false; // notify-enabled state is edge-detected in loop()
Serial.println("PHONE CONNECTED");
}
void prph_disconnect_callback(uint16_t conn_handle, uint8_t reason)
{
(void) conn_handle;
(void) reason;
phoneConnected = false;
phoneReady = false;
phoneConnHandle = BLE_CONN_HANDLE_INVALID;
Serial.println("PHONE DISCONNECTED");
}
// ============================================================================
// ADVERTISEMENT NAME PARSING
// ============================================================================
// Walks the raw AD structures in an advertisement/scan-response payload
// looking for the Complete or Shortened Local Name field. Pure read of
// data already delivered in the report — no GATT connection needed.
bool extractAdvName(const ble_gap_evt_adv_report_t* report, char* outName, size_t outSize)
{
const uint8_t* data = report->data.p_data;
uint16_t len = report->data.len;
uint16_t offset = 0;
while (offset + 1 < len)
{
uint8_t fieldLen = data[offset];
if (fieldLen == 0) break;
if (offset + 1 + fieldLen > len) break;
uint8_t adType = data[offset + 1];
if (adType == BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME ||
adType == BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME)
{
uint8_t nameLen = fieldLen - 1;
if (nameLen >= outSize) nameLen = (uint8_t)(outSize - 1);
memcpy(outName, &data[offset + 2], nameLen);
outName[nameLen] = '\0';
return true;
}
offset = (uint16_t)(offset + 1 + fieldLen);
}
return false;
}
bool addrEquals(const ble_gap_addr_t& a, const ble_gap_addr_t& b)
{
if (a.addr_type != b.addr_type) return false;
return memcmp(a.addr, b.addr, BLE_GAP_ADDR_LEN) == 0;
}
// ============================================================================
// BOOT BANNER / RESET REASON
// ============================================================================
void printBootBanner()
{
Serial.println("========================");
Serial.println("BOXING HUB BOOT");
Serial.println("========================");
Serial.print("Build date: ");
Serial.println(__DATE__);
Serial.print("Build time: ");
Serial.println(__TIME__);
Serial.println("========================");
}
void printResetReason()
{
uint32_t resetReas = NRF_POWER->RESETREAS;
Serial.print("Reset reason: ");
if (resetReas == 0)
{
Serial.print("POWERON ");
}
if (resetReas & POWER_RESETREAS_RESETPIN_Msk) Serial.print("RESETPIN ");
if (resetReas & POWER_RESETREAS_DOG_Msk) Serial.print("WATCHDOG ");
if (resetReas & POWER_RESETREAS_LOCKUP_Msk) Serial.print("LOCKUP ");
if (resetReas & POWER_RESETREAS_SREQ_Msk) Serial.print("SREQ ");
if (resetReas & POWER_RESETREAS_OFF_Msk) Serial.print("OFF ");
if (resetReas & POWER_RESETREAS_LPCOMP_Msk) Serial.print("LPCOMP ");
if (resetReas & POWER_RESETREAS_DIF_Msk) Serial.print("DIF ");
if (resetReas & POWER_RESETREAS_NFC_Msk) Serial.print("NFC ");
if (resetReas & POWER_RESETREAS_VBUS_Msk) Serial.print("VBUS ");
Serial.println();
// Nordic-recommended clear: writing back exactly the bits read clears
// only those bits.
NRF_POWER->RESETREAS = resetReas;
}
// ============================================================================
// LED
// ============================================================================
// Board LED is active-LOW; analogWrite lets us mix real colors (e.g.
// orange) instead of being limited to the 8 on/off combinations.
void setLedColor(uint8_t r, uint8_t g, uint8_t b)
{
analogWrite(LED_RED, 255 - r);
analogWrite(LED_GREEN, 255 - g);
analogWrite(LED_BLUE, 255 - b);
}
void triggerDiscoveryFailureBlink()
{
discoveryFailureBlinkUntilMs = millis() + 2000; // blink orange for 2s
}
void updateLed()
{
uint32_t now = millis();
bool blinkOn = ((now / 200) % 2) == 0;
// Priority 1: transient discovery-failure indication.
if (now < discoveryFailureBlinkUntilMs)
{
setLedColor(blinkOn ? 255 : 0, blinkOn ? 140 : 0, 0); // orange blink
return;
}
bool anyGloveReady = leftLink.ready || rightLink.ready;
// Priority 2: phone disconnected while at least one glove is ready.
if (!phoneConnected && anyGloveReady)
{
setLedColor(blinkOn ? 255 : 0, 0, 0); // red blink
return;
}
// Priority 3: both gloves ready.
if (leftLink.ready && rightLink.ready)
{
setLedColor(0, 255, 0); // solid green
return;
}
// Priority 4: exactly one glove ready.
if (anyGloveReady)
{
setLedColor(255, 255, 0); // solid yellow
return;
}
// Default: still scanning for gloves.
setLedColor(0, 0, 255); // solid blue
}
// ============================================================================
// DEEP SLEEP — enter after DEEP_SLEEP_TIMEOUT_MS with no significant
// motion on the bag IMU. Tears down all three links (both gloves + phone),
// arms the bag IMU's own wake-up-on-motion interrupt on INT1, then powers
// the nRF52840 off via the SoftDevice-safe call. Waking is a full reboot
// (setup() runs again) — scanning/advertising/reconnection all resume
// exactly as they do on a cold boot.
// ============================================================================
void enterDeepSleep()
{
Serial.println("No motion for 5 min — entering deep sleep");
Serial.flush();
// Stop scanning and disconnect any connected/connecting gloves
Bluefruit.Scanner.stop();
if (leftLink.connHandle != BLE_CONN_HANDLE_INVALID)
{
Bluefruit.disconnect(leftLink.connHandle);
}
if (rightLink.connHandle != BLE_CONN_HANDLE_INVALID)
{
Bluefruit.disconnect(rightLink.connHandle);
}
// Stop advertising and disconnect the phone
Bluefruit.Advertising.stop();
if (phoneConnHandle != BLE_CONN_HANDLE_INVALID)
{
Bluefruit.disconnect(phoneConnHandle);
}
delay(100);
// Configure bag IMU wake-up-on-motion, routed to INT1
bagImu.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_DUR, 0x00); // no extra duration
bagImu.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x02); // sensitivity, tune 1-63
bagImu.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x80); // enable interrupts, no latch
bagImu.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x20); // wake-up event -> INT1
bagImu.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x10); // low-power accel, 12.5Hz
delay(50); // let settings take effect before power-off
// Arm INT1 (P0.11) as the System OFF wake source
nrf_gpio_cfg_sense_input(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1),
NRF_GPIO_PIN_PULLDOWN,
NRF_GPIO_PIN_SENSE_HIGH);
// SoftDevice-safe power off (NOT NRF_POWER->SYSTEMOFF directly — SD owns POWER)
sd_power_system_off();
}
// ============================================================================
// FREE MEMORY (this core's FreeRTOS port wraps malloc()/free() from newlib
// instead of compiling in heap_4.c, so xPortGetFreeHeapSize() does not
// exist to link against here. This is the standard fallback: the gap
// between the current stack pointer and the current top of the malloc
// heap (sbrk(0)) is real, measured free space.)
// ============================================================================
extern "C" char *sbrk(int i);
static uint32_t freeMemoryBytes()
{
char stackTop;
return (uint32_t)(&stackTop - reinterpret_cast<char*>(sbrk(0)));
}
// ============================================================================
// DIAGNOSTICS — printed once per second
// ============================================================================
void printDiagnostics()
{
Serial.println("---- DIAG ----");
Serial.print("LEFT_CONNECTED: ");
Serial.println(leftLink.connected ? "yes" : "no");
Serial.print("LEFT_READY: ");
Serial.println(leftLink.ready ? "yes" : "no");
Serial.print("LEFT_PACKETS_PER_SEC: ");
Serial.println(leftLink.packetsThisSec);
Serial.print("RIGHT_CONNECTED: ");
Serial.println(rightLink.connected ? "yes" : "no");
Serial.print("RIGHT_READY: ");
Serial.println(rightLink.ready ? "yes" : "no");
Serial.print("RIGHT_PACKETS_PER_SEC: ");
Serial.println(rightLink.packetsThisSec);
Serial.print("PHONE_CONNECTED: ");
Serial.println(phoneConnected ? "yes" : "no");
Serial.print("PHONE_NOTIFY_ENABLED: ");
Serial.println(phoneReady ? "yes" : "no");
Serial.print("SCAN_RUNNING: ");
Serial.println(Bluefruit.Scanner.isRunning() ? "yes" : "no");
Serial.print("BAG_IMU_READS_PER_SEC: ");
Serial.println(bagImuReadsThisSec);
Serial.print("BAG_PACKETS_SENT_TOTAL: ");
Serial.println(bagPacketsSentTotal);
Serial.print("BAG_PACKETS_SENT_PER_SEC: ");
Serial.println(bagPacketsSentThisSec);
Serial.print("BAG_PACKETS_SKIPPED_PHONE_NOT_READY: ");
Serial.println(bagPacketsSkippedThisSec);
Serial.print("FREE_HEAP: ");
Serial.println(freeMemoryBytes());
if (leftLink.ready && leftLink.rssiMonitoring)
{
BLEConnection* c = Bluefruit.Connection(leftLink.connHandle);
Serial.print("RSSI_LEFT: ");
Serial.println(c ? c->getRssi() : 0);
}
else
{
Serial.println("RSSI_LEFT: unavailable");
}
if (rightLink.ready && rightLink.rssiMonitoring)
{
BLEConnection* c = Bluefruit.Connection(rightLink.connHandle);
Serial.print("RSSI_RIGHT: ");
Serial.println(c ? c->getRssi() : 0);
}
else
{
Serial.println("RSSI_RIGHT: unavailable");
}
Serial.println("--------------");
}
Regarding your low RSSI, that may be related to antenna placement, interference or orientation.
I’ll try your code again (this version) to compare with your results.
As an aside, I have a similar project but don’t use a “BLE Advertise - Connect” paradigm.
I use a method where the data is passed in a non-connectable advertisement. In your case, since the “packet data” is 20 bytes, legacy advertising can be used.
“that may be related to antenna placement, interference or orientation.”
yes, these were the firsts I rules out.
I took 1 sensor out, so now sensor (peripherial) >> sensor (peripherial + Central) >> telephone, and now it looks like the dBm improved. Maybe the antenna can’t handle this many sensors at the same time.
can you tell me about it more? non-connectable advertisement? so you don’t actually connect, you just took that data from the advertisement constantly?