798 lines
32 KiB
C++
798 lines
32 KiB
C++
/*
|
|
* IMU BLE Mouse — Seeed XIAO nRF52840 Sense (v3.4)
|
|
* ================================================================
|
|
* Feature flags — comment out any line to disable that feature.
|
|
* ATT table size is computed automatically from enabled features.
|
|
* Start with minimal flags to isolate the SoftDevice RAM issue,
|
|
* then re-enable one at a time.
|
|
*
|
|
* MINIMUM (just working mouse, no BLE config):
|
|
* leave only FEATURE_BATTERY_MONITOR + FEATURE_BOOT_LOOP_DETECT
|
|
*
|
|
* RECOMMENDED first test:
|
|
* enable FEATURE_CONFIG_SERVICE, keep TAP + STREAM + TELEMETRY off
|
|
*
|
|
* ── Feature flag index ───────────────────────────────────────────
|
|
* FEATURE_CONFIG_SERVICE Custom GATT service (ConfigBlob + Command)
|
|
* FEATURE_TELEMETRY +24-byte notify characteristic, 1 Hz
|
|
* FEATURE_IMU_STREAM +14-byte notify characteristic, ~100 Hz
|
|
* FEATURE_TAP_DETECTION LSM6DS3 hardware tap engine → L/R clicks
|
|
* FEATURE_TEMP_COMPENSATION Gyro drift correction by temperature delta
|
|
* FEATURE_AUTO_RECAL Recalibrate after AUTO_RECAL_MS idle
|
|
* FEATURE_BATTERY_MONITOR ADC battery read + BLE Battery Service
|
|
* FEATURE_BOOT_LOOP_DETECT .noinit crash counter → safe mode
|
|
*
|
|
* Dependencies:
|
|
* FEATURE_TELEMETRY requires FEATURE_CONFIG_SERVICE
|
|
* FEATURE_IMU_STREAM requires FEATURE_CONFIG_SERVICE
|
|
* ================================================================
|
|
*/
|
|
|
|
// ─── Feature Flags ────────────────────────────────────────────────────────────
|
|
#define FEATURE_CONFIG_SERVICE
|
|
#define FEATURE_TELEMETRY
|
|
#define FEATURE_IMU_STREAM
|
|
#define FEATURE_TAP_DETECTION
|
|
#define FEATURE_TEMP_COMPENSATION
|
|
#define FEATURE_AUTO_RECAL
|
|
#define FEATURE_BATTERY_MONITOR
|
|
#define FEATURE_BOOT_LOOP_DETECT
|
|
|
|
// ─── Debug ────────────────────────────────────────────────────────────────────
|
|
// #define DEBUG
|
|
|
|
// ─── ATT table size ───────────────────────────────────────────────────────────
|
|
// Must be passed to configAttrTableSize() BEFORE Bluefruit.begin().
|
|
// Too small → SoftDevice panics. Too large → SoftDevice claims RAM the linker
|
|
// already assigned to the app and also panics. Sweet spot is 1500-2000.
|
|
//
|
|
// HID service alone costs ~700 B (report map + 6 characteristics + CCCDs).
|
|
// Add DIS (~100 B), BAS (~50 B), then our custom characteristics.
|
|
// Use 1536 as the safe base; add 256 per notify characteristic.
|
|
//
|
|
// Feature cost breakdown:
|
|
// HID + DIS + BAS baseline : ~900 B (always present)
|
|
// CONFIG_SERVICE (blob+cmd) : ~80 B
|
|
// TELEMETRY (R/N 24 bytes) : ~40 B
|
|
// IMU_STREAM (N 14 bytes) : ~30 B
|
|
//
|
|
#define _ATT_BASE 900
|
|
#ifdef FEATURE_CONFIG_SERVICE
|
|
#define _ATT_CFG 80
|
|
#else
|
|
#define _ATT_CFG 0
|
|
#endif
|
|
#ifdef FEATURE_TELEMETRY
|
|
#define _ATT_TELEM 40
|
|
#else
|
|
#define _ATT_TELEM 0
|
|
#endif
|
|
#ifdef FEATURE_IMU_STREAM
|
|
#define _ATT_STREAM 30
|
|
#else
|
|
#define _ATT_STREAM 0
|
|
#endif
|
|
// Floor of 1536 so we never go below what HID actually needs
|
|
#define ATT_TABLE_SIZE_CALC (_ATT_BASE + _ATT_CFG + _ATT_TELEM + _ATT_STREAM)
|
|
#define ATT_TABLE_SIZE (ATT_TABLE_SIZE_CALC < 1536 ? 1536 : ATT_TABLE_SIZE_CALC)
|
|
|
|
// ─── Includes ─────────────────────────────────────────────────────────────────
|
|
#include <bluefruit.h>
|
|
#include <Adafruit_LittleFS.h>
|
|
#include <InternalFileSystem.h>
|
|
#include "LSM6DS3.h"
|
|
#include "Wire.h"
|
|
|
|
// ─── Boot-loop detection ──────────────────────────────────────────────────────
|
|
#ifdef FEATURE_BOOT_LOOP_DETECT
|
|
static uint32_t __attribute__((section(".noinit"))) bootCount;
|
|
static uint32_t __attribute__((section(".noinit"))) bootMagic;
|
|
#endif
|
|
static bool safeMode = false;
|
|
static bool bootCountCleared = false;
|
|
|
|
// ─── BLE Standard Services ────────────────────────────────────────────────────
|
|
BLEDis bledis;
|
|
BLEHidAdafruit blehid;
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
BLEBas blebas;
|
|
#endif
|
|
|
|
// ─── BLE Config Service ───────────────────────────────────────────────────────
|
|
#ifdef FEATURE_CONFIG_SERVICE
|
|
BLEService cfgService (0x1234);
|
|
BLECharacteristic cfgBlob (0x1235); // ConfigBlob R/W 16 bytes
|
|
BLECharacteristic cfgCommand (0x1236); // Command W 1 byte
|
|
#ifdef FEATURE_TELEMETRY
|
|
BLECharacteristic cfgTelemetry(0x1237); // Telemetry R/N 24 bytes
|
|
#endif
|
|
#ifdef FEATURE_IMU_STREAM
|
|
BLECharacteristic cfgImuStream(0x1238); // ImuStream N 14 bytes
|
|
#endif
|
|
#endif
|
|
|
|
// ─── IMU ──────────────────────────────────────────────────────────────────────
|
|
LSM6DS3 imu(I2C_MODE, 0x6A);
|
|
|
|
#define REG_CTRL1_XL 0x10
|
|
#define REG_TAP_CFG 0x58
|
|
#define REG_TAP_THS_6D 0x59
|
|
#define REG_INT_DUR2 0x5A
|
|
#define REG_WAKE_UP_THS 0x5B
|
|
#define REG_MD1_CFG 0x5E
|
|
#define REG_TAP_SRC 0x1C
|
|
#define REG_OUT_TEMP_L 0x20
|
|
#define REG_OUT_TEMP_H 0x21
|
|
|
|
// ─── Pins ─────────────────────────────────────────────────────────────────────
|
|
#define PIN_VBAT_ENABLE (14)
|
|
#define PIN_VBAT_READ (32)
|
|
#define PIN_CHG (23)
|
|
#define PIN_HICHG (22)
|
|
|
|
// ─── Persistence ──────────────────────────────────────────────────────────────
|
|
#define CONFIG_FILENAME "/imu_mouse_cfg.bin"
|
|
#define CONFIG_MAGIC 0xDEAD1239UL
|
|
|
|
using namespace Adafruit_LittleFS_Namespace;
|
|
File cfgFile(InternalFS);
|
|
|
|
// ─── Enums ────────────────────────────────────────────────────────────────────
|
|
enum CurveType : uint8_t { CURVE_LINEAR=0, CURVE_SQUARE=1, CURVE_SQRT=2 };
|
|
enum ChargeMode : uint8_t { CHARGE_OFF=0, CHARGE_SLOW=1, CHARGE_FAST=2 };
|
|
enum ChargeStatus: uint8_t { CHGSTAT_DISCHARGING=0, CHGSTAT_CHARGING=1, CHGSTAT_FULL=2 };
|
|
|
|
// ─── Config (stored in flash) ─────────────────────────────────────────────────
|
|
struct Config {
|
|
uint32_t magic;
|
|
float sensitivity;
|
|
float deadZone;
|
|
float accelStrength;
|
|
CurveType curve;
|
|
uint8_t axisFlip;
|
|
ChargeMode chargeMode;
|
|
};
|
|
Config cfg;
|
|
const Config CFG_DEFAULTS = {
|
|
CONFIG_MAGIC, 600.0f, 0.060f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW
|
|
};
|
|
|
|
// ─── ConfigBlob (over BLE, no magic) ─────────────────────────────────────────
|
|
struct __attribute__((packed)) ConfigBlob {
|
|
float sensitivity; // [0]
|
|
float deadZone; // [4]
|
|
float accelStrength; // [8]
|
|
uint8_t curve; // [12]
|
|
uint8_t axisFlip; // [13]
|
|
uint8_t chargeMode; // [14]
|
|
uint8_t _pad; // [15]
|
|
};
|
|
static_assert(sizeof(ConfigBlob) == 16, "ConfigBlob must be 16 bytes");
|
|
|
|
// ─── TelemetryPacket ──────────────────────────────────────────────────────────
|
|
#ifdef FEATURE_TELEMETRY
|
|
struct __attribute__((packed)) TelemetryPacket {
|
|
uint32_t uptimeSeconds; // [0]
|
|
uint32_t leftClicks; // [4]
|
|
uint32_t rightClicks; // [8]
|
|
float tempCelsius; // [12]
|
|
float biasRmsRadS; // [16]
|
|
uint16_t recalCount; // [20]
|
|
uint8_t chargeStatus; // [22]
|
|
uint8_t _pad; // [23]
|
|
};
|
|
static_assert(sizeof(TelemetryPacket) == 24, "TelemetryPacket must be 24 bytes");
|
|
TelemetryPacket telem = {};
|
|
#endif
|
|
|
|
// ─── ImuPacket ────────────────────────────────────────────────────────────────
|
|
#ifdef FEATURE_IMU_STREAM
|
|
struct __attribute__((packed)) ImuPacket {
|
|
int16_t gyroY_mDPS; // [0]
|
|
int16_t gyroZ_mDPS; // [2]
|
|
int16_t accelX_mg; // [4]
|
|
int16_t accelY_mg; // [6]
|
|
int16_t accelZ_mg; // [8]
|
|
int8_t moveX; // [10]
|
|
int8_t moveY; // [11]
|
|
uint8_t flags; // [12] bit0=idle bit1=singleTap bit2=doubleTap
|
|
uint8_t _pad; // [13]
|
|
};
|
|
static_assert(sizeof(ImuPacket) == 14, "ImuPacket must be 14 bytes");
|
|
#endif
|
|
|
|
// ─── Tuning constants ─────────────────────────────────────────────────────────
|
|
const float ALPHA = 0.96f;
|
|
const int LOOP_RATE_MS = 10;
|
|
const int BIAS_SAMPLES = 200;
|
|
const int IDLE_FRAMES = 150;
|
|
const unsigned long BATT_REPORT_MS = 10000;
|
|
const unsigned long TELEMETRY_MS = 1000;
|
|
const unsigned long HEARTBEAT_MS = 2000;
|
|
const int HEARTBEAT_DUR = 30;
|
|
const unsigned long BOOT_SAFE_MS = 5000;
|
|
#ifdef FEATURE_IMU_STREAM
|
|
const unsigned long IMU_STREAM_RATE_MS = 50; // 20 Hz max — 100 Hz overwhelms BLE conn interval
|
|
#endif
|
|
const float BATT_FULL = 4.20f;
|
|
const float BATT_EMPTY = 3.00f;
|
|
const float BATT_CRITICAL = 3.10f;
|
|
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
const unsigned long CLICK_HOLD_MS = 60;
|
|
#endif
|
|
#ifdef FEATURE_TEMP_COMPENSATION
|
|
const float TEMP_COMP_COEFF_DPS_C = 0.004f;
|
|
#endif
|
|
#ifdef FEATURE_AUTO_RECAL
|
|
const unsigned long AUTO_RECAL_MS = 5UL * 60UL * 1000UL;
|
|
#endif
|
|
|
|
// ─── State ────────────────────────────────────────────────────────────────────
|
|
float angleX = 0, angleY = 0;
|
|
float accumX = 0, accumY = 0;
|
|
float biasGX = 0, biasGY = 0, biasGZ = 0;
|
|
float calTempC = 25.0f;
|
|
float cachedTempC = 25.0f;
|
|
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
bool clickButtonDown = false;
|
|
uint8_t clickButton = 0;
|
|
unsigned long clickDownMs= 0;
|
|
uint32_t statLeftClicks = 0;
|
|
uint32_t statRightClicks = 0;
|
|
#endif
|
|
|
|
#ifdef FEATURE_IMU_STREAM
|
|
bool imuStreamEnabled = false;
|
|
#endif
|
|
|
|
bool pendingCal = false;
|
|
bool pendingReset = false;
|
|
|
|
ChargeStatus lastChargeStatus = CHGSTAT_DISCHARGING;
|
|
|
|
int idleFrames = 0;
|
|
unsigned long idleStartMs = 0;
|
|
unsigned long lastTime = 0;
|
|
unsigned long lastBattTime = 0;
|
|
unsigned long lastHeartbeat = 0;
|
|
unsigned long lastTelemetry = 0;
|
|
unsigned long bootStartMs = 0;
|
|
#ifdef FEATURE_IMU_STREAM
|
|
unsigned long lastImuStream = 0;
|
|
#endif
|
|
|
|
#ifdef FEATURE_TELEMETRY
|
|
uint16_t statRecalCount = 0;
|
|
float statBiasRms = 0.0f;
|
|
#endif
|
|
|
|
// ─── I2C helpers ──────────────────────────────────────────────────────────────
|
|
void imuWriteReg(uint8_t reg, uint8_t val) {
|
|
// LSM6DS3 is on Wire1 (internal I2C, SDA=P0.17, SCL=P0.16), NOT Wire (external pins 4/5)
|
|
Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.write(val); Wire1.endTransmission();
|
|
}
|
|
uint8_t imuReadReg(uint8_t reg) {
|
|
Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.endTransmission(false);
|
|
Wire1.requestFrom((uint8_t)0x6A, (uint8_t)1);
|
|
return Wire1.available() ? Wire1.read() : 0;
|
|
}
|
|
|
|
// ─── Temperature ──────────────────────────────────────────────────────────────
|
|
float readIMUTemp() {
|
|
int16_t raw = (int16_t)((imuReadReg(REG_OUT_TEMP_H) << 8) | imuReadReg(REG_OUT_TEMP_L));
|
|
return 25.0f + (float)raw / 256.0f;
|
|
}
|
|
|
|
// ─── Tap detection ────────────────────────────────────────────────────────────
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
void setupTapDetection() {
|
|
imuWriteReg(REG_CTRL1_XL, 0x60); // ODR=416Hz, FS=±2g
|
|
imuWriteReg(REG_TAP_CFG, 0x8E); // INT_EN + LIR + TAP_Z/Y/X
|
|
imuWriteReg(REG_TAP_THS_6D, 0x0C); // threshold 750 mg (was 500 mg — too easy to false-trigger)
|
|
imuWriteReg(REG_INT_DUR2, 0x7A); // DUR=7(538ms), QUIET=2(19ms), SHOCK=2(38ms)
|
|
imuWriteReg(REG_WAKE_UP_THS, 0x80); // enable double-tap
|
|
imuWriteReg(REG_MD1_CFG, 0x48); // route taps to INT1
|
|
Serial.println("[TAP] Engine configured — single=LEFT, double=RIGHT");
|
|
}
|
|
|
|
void processTaps(unsigned long now) {
|
|
// Release held button after CLICK_HOLD_MS
|
|
if (clickButtonDown && (now - clickDownMs >= CLICK_HOLD_MS)) {
|
|
blehid.mouseButtonPress(clickButton, false);
|
|
clickButtonDown = false; clickButton = 0;
|
|
}
|
|
if (clickButtonDown) return; // Don't start a new click while one is held
|
|
|
|
// The LSM6DS3 (with D_TAP_EN) already disambiguates at hardware level:
|
|
// SINGLE_TAP is only set after the DUR window expires with no second tap.
|
|
// DOUBLE_TAP is set immediately when the second tap arrives within DUR.
|
|
// We trust this directly — no software delay needed.
|
|
uint8_t tapSrc = imuReadReg(REG_TAP_SRC);
|
|
if (!(tapSrc & 0x40)) return; // TAP_IA not set — no event
|
|
|
|
if (tapSrc & 0x10) { // DOUBLE_TAP → right click
|
|
Serial.println("[TAP] Double → RIGHT");
|
|
blehid.mouseButtonPress(MOUSE_BUTTON_RIGHT, true);
|
|
clickButton = MOUSE_BUTTON_RIGHT; clickButtonDown = true; clickDownMs = now;
|
|
statRightClicks++;
|
|
} else if (tapSrc & 0x20) { // SINGLE_TAP → left click
|
|
Serial.println("[TAP] Single → LEFT");
|
|
blehid.mouseButtonPress(MOUSE_BUTTON_LEFT, true);
|
|
clickButton = MOUSE_BUTTON_LEFT; clickButtonDown = true; clickDownMs = now;
|
|
statLeftClicks++;
|
|
}
|
|
}
|
|
#endif // FEATURE_TAP_DETECTION
|
|
|
|
// ─── Charge mode ──────────────────────────────────────────────────────────────
|
|
void applyChargeMode(ChargeMode mode) {
|
|
switch (mode) {
|
|
case CHARGE_OFF: pinMode(PIN_HICHG, INPUT_PULLUP); break;
|
|
case CHARGE_SLOW: pinMode(PIN_HICHG, OUTPUT); digitalWrite(PIN_HICHG, HIGH); break;
|
|
case CHARGE_FAST: pinMode(PIN_HICHG, OUTPUT); digitalWrite(PIN_HICHG, LOW); break;
|
|
}
|
|
const char* n[] = {"OFF (~0mA)", "SLOW (50mA)", "FAST (100mA)"};
|
|
Serial.print("[CHG] "); Serial.println(n[mode]);
|
|
}
|
|
|
|
// ─── Config persistence ───────────────────────────────────────────────────────
|
|
void loadConfig() {
|
|
InternalFS.begin();
|
|
cfgFile.open(CONFIG_FILENAME, FILE_O_READ);
|
|
if (cfgFile) {
|
|
cfgFile.read(&cfg, sizeof(cfg)); cfgFile.close();
|
|
if (cfg.magic != CONFIG_MAGIC) {
|
|
cfg = CFG_DEFAULTS; Serial.println("[CFG] Defaults (bad magic)");
|
|
} else {
|
|
Serial.println("[CFG] Loaded from flash");
|
|
}
|
|
} else { cfg = CFG_DEFAULTS; Serial.println("[CFG] Defaults (no file)"); }
|
|
}
|
|
|
|
void saveConfig() {
|
|
InternalFS.remove(CONFIG_FILENAME);
|
|
cfgFile.open(CONFIG_FILENAME, FILE_O_WRITE);
|
|
if (cfgFile) { cfgFile.write((uint8_t*)&cfg, sizeof(cfg)); cfgFile.close(); Serial.println("[CFG] Saved"); }
|
|
else { Serial.println("[CFG] ERROR: write failed"); }
|
|
}
|
|
|
|
// ─── ConfigBlob push ─────────────────────────────────────────────────────────
|
|
#ifdef FEATURE_CONFIG_SERVICE
|
|
void pushConfigBlob() {
|
|
ConfigBlob b;
|
|
b.sensitivity = cfg.sensitivity; b.deadZone = cfg.deadZone;
|
|
b.accelStrength = cfg.accelStrength; b.curve = (uint8_t)cfg.curve;
|
|
b.axisFlip = cfg.axisFlip; b.chargeMode = (uint8_t)cfg.chargeMode; b._pad = 0;
|
|
cfgBlob.write((uint8_t*)&b, sizeof(b));
|
|
}
|
|
#endif
|
|
|
|
void factoryReset() {
|
|
cfg = CFG_DEFAULTS; saveConfig();
|
|
applyChargeMode(cfg.chargeMode);
|
|
#ifdef FEATURE_CONFIG_SERVICE
|
|
if (!safeMode) pushConfigBlob();
|
|
#endif
|
|
#ifdef FEATURE_TELEMETRY
|
|
telem = {};
|
|
#endif
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
statLeftClicks = statRightClicks = 0;
|
|
#endif
|
|
Serial.println("[CFG] Factory reset complete");
|
|
}
|
|
|
|
// ─── BLE callbacks ────────────────────────────────────────────────────────────
|
|
#ifdef FEATURE_CONFIG_SERVICE
|
|
void onConfigBlobWrite(uint16_t h, BLECharacteristic* c, uint8_t* d, uint16_t l) {
|
|
if (l != sizeof(ConfigBlob)) { Serial.println("[CFG] Bad blob length"); return; }
|
|
ConfigBlob* b = (ConfigBlob*)d;
|
|
cfg.sensitivity = b->sensitivity;
|
|
cfg.deadZone = b->deadZone;
|
|
cfg.accelStrength = b->accelStrength;
|
|
if (b->curve <= 2) cfg.curve = (CurveType)b->curve;
|
|
cfg.axisFlip = b->axisFlip;
|
|
if (b->chargeMode <= 2) { cfg.chargeMode = (ChargeMode)b->chargeMode; applyChargeMode(cfg.chargeMode); }
|
|
saveConfig();
|
|
Serial.print("[CFG] Written — sens="); Serial.print(cfg.sensitivity,0);
|
|
Serial.print(" dz="); Serial.print(cfg.deadZone,3);
|
|
Serial.print(" curve="); Serial.print(cfg.curve);
|
|
Serial.print(" chg="); Serial.println(cfg.chargeMode);
|
|
}
|
|
|
|
void onCommandWrite(uint16_t h, BLECharacteristic* c, uint8_t* d, uint16_t l) {
|
|
if (l < 1) return;
|
|
if (d[0] == 0x01) pendingCal = true;
|
|
if (d[0] == 0xFF) pendingReset = true;
|
|
}
|
|
|
|
#ifdef FEATURE_IMU_STREAM
|
|
void onImuStreamCccd(uint16_t conn_hdl, BLECharacteristic* chr, uint16_t value) {
|
|
imuStreamEnabled = (value == BLE_GATT_HVX_NOTIFICATION);
|
|
Serial.print("[STREAM] "); Serial.println(imuStreamEnabled ? "ON" : "OFF");
|
|
}
|
|
#endif
|
|
#endif // FEATURE_CONFIG_SERVICE
|
|
|
|
// ─── BLE service setup ────────────────────────────────────────────────────────
|
|
#ifdef FEATURE_CONFIG_SERVICE
|
|
void setupConfigService() {
|
|
cfgService.begin();
|
|
|
|
cfgBlob.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
|
|
cfgBlob.setPermission(SECMODE_OPEN, SECMODE_OPEN);
|
|
cfgBlob.setFixedLen(sizeof(ConfigBlob));
|
|
cfgBlob.setWriteCallback(onConfigBlobWrite);
|
|
cfgBlob.begin();
|
|
pushConfigBlob();
|
|
|
|
cfgCommand.setProperties(CHR_PROPS_WRITE);
|
|
cfgCommand.setPermission(SECMODE_OPEN, SECMODE_OPEN);
|
|
cfgCommand.setFixedLen(1);
|
|
cfgCommand.setWriteCallback(onCommandWrite);
|
|
cfgCommand.begin();
|
|
|
|
#ifdef FEATURE_TELEMETRY
|
|
cfgTelemetry.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
|
|
cfgTelemetry.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
|
|
cfgTelemetry.setFixedLen(sizeof(TelemetryPacket));
|
|
cfgTelemetry.begin();
|
|
cfgTelemetry.write((uint8_t*)&telem, sizeof(telem));
|
|
#endif
|
|
|
|
#ifdef FEATURE_IMU_STREAM
|
|
cfgImuStream.setProperties(CHR_PROPS_NOTIFY);
|
|
cfgImuStream.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
|
|
cfgImuStream.setFixedLen(sizeof(ImuPacket));
|
|
cfgImuStream.setCccdWriteCallback(onImuStreamCccd);
|
|
cfgImuStream.begin();
|
|
#endif
|
|
|
|
// Print actual ATT table budget at runtime
|
|
Serial.print("[BLE] ATT_TABLE_SIZE="); Serial.print(ATT_TABLE_SIZE);
|
|
Serial.print(" | chars=2");
|
|
#ifdef FEATURE_TELEMETRY
|
|
Serial.print("+TELEM");
|
|
#endif
|
|
#ifdef FEATURE_IMU_STREAM
|
|
Serial.print("+STREAM");
|
|
#endif
|
|
Serial.println();
|
|
}
|
|
#endif // FEATURE_CONFIG_SERVICE
|
|
|
|
// ─── Battery ──────────────────────────────────────────────────────────────────
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
float readBatteryVoltage() {
|
|
pinMode(PIN_VBAT_ENABLE, OUTPUT); digitalWrite(PIN_VBAT_ENABLE, LOW); delay(1);
|
|
pinMode(PIN_VBAT_READ, INPUT);
|
|
analogReference(AR_INTERNAL_3_0); analogReadResolution(12);
|
|
for (int i=0; i<5; i++) { analogRead(PIN_VBAT_READ); delay(1); }
|
|
int32_t raw=0; for (int i=0; i<16; i++) raw += analogRead(PIN_VBAT_READ); raw /= 16;
|
|
digitalWrite(PIN_VBAT_ENABLE, HIGH);
|
|
analogReference(AR_DEFAULT); analogReadResolution(10);
|
|
return (raw / 4096.0f) * 3.0f * 2.0f;
|
|
}
|
|
int batteryPercent(float v) {
|
|
return (int)constrain((v - BATT_EMPTY) / (BATT_FULL - BATT_EMPTY) * 100.f, 0, 100);
|
|
}
|
|
|
|
void updateBattery() {
|
|
float v = readBatteryVoltage(); int pct = batteryPercent(v);
|
|
bool chg = (digitalRead(PIN_CHG) == LOW);
|
|
ChargeStatus status = chg ? (pct >= 99 ? CHGSTAT_FULL : CHGSTAT_CHARGING) : CHGSTAT_DISCHARGING;
|
|
blebas.write(pct);
|
|
lastChargeStatus = status;
|
|
#ifdef FEATURE_TELEMETRY
|
|
telem.chargeStatus = (uint8_t)status;
|
|
#endif
|
|
const char* st[] = {"discharging","charging","full"};
|
|
Serial.print("[BATT] "); Serial.print(v,2); Serial.print("V ");
|
|
Serial.print(pct); Serial.print("% "); Serial.println(st[status]);
|
|
if (status == CHGSTAT_DISCHARGING && v < BATT_CRITICAL)
|
|
for (int i=0; i<6; i++) { digitalWrite(LED_RED,LOW); delay(80); digitalWrite(LED_RED,HIGH); delay(80); }
|
|
}
|
|
#endif // FEATURE_BATTERY_MONITOR
|
|
|
|
// ─── Calibration ─────────────────────────────────────────────────────────────
|
|
void calibrateGyroBias() {
|
|
Serial.println("[CAL] Hold still...");
|
|
double sx=0, sy=0, sz=0;
|
|
for (int i=0; i<BIAS_SAMPLES; i++) {
|
|
sx += imu.readFloatGyroX(); sy += imu.readFloatGyroY(); sz += imu.readFloatGyroZ();
|
|
digitalWrite(LED_BLUE, (i%20 < 10)); delay(5);
|
|
}
|
|
biasGX = (float)(sx/BIAS_SAMPLES);
|
|
biasGY = (float)(sy/BIAS_SAMPLES);
|
|
biasGZ = (float)(sz/BIAS_SAMPLES);
|
|
calTempC = readIMUTemp();
|
|
angleX = angleY = accumX = accumY = 0.0f;
|
|
|
|
#ifdef FEATURE_TELEMETRY
|
|
statRecalCount++;
|
|
float bxr = biasGX*(PI/180.f), byr = biasGY*(PI/180.f), bzr = biasGZ*(PI/180.f);
|
|
statBiasRms = sqrtf((bxr*bxr + byr*byr + bzr*bzr) / 3.0f);
|
|
#endif
|
|
|
|
digitalWrite(LED_BLUE, HIGH);
|
|
Serial.print("[CAL] T="); Serial.print(calTempC,1);
|
|
Serial.print("C bias="); Serial.print(biasGX,4);
|
|
Serial.print(","); Serial.print(biasGY,4);
|
|
Serial.print(","); Serial.println(biasGZ,4);
|
|
}
|
|
|
|
// ─── Motion curve ─────────────────────────────────────────────────────────────
|
|
float applyCurve(float v) {
|
|
switch (cfg.curve) {
|
|
case CURVE_SQUARE: return (v >= 0 ? 1.f : -1.f) * v * v;
|
|
case CURVE_SQRT: return (v >= 0 ? 1.f : -1.f) * sqrtf(fabsf(v));
|
|
default: return v;
|
|
}
|
|
}
|
|
float applyAcceleration(float d) { return d * (1.0f + fabsf(d) * cfg.accelStrength); }
|
|
|
|
// ─── Telemetry push ───────────────────────────────────────────────────────────
|
|
#ifdef FEATURE_TELEMETRY
|
|
void pushTelemetry(unsigned long now) {
|
|
telem.uptimeSeconds = now / 1000;
|
|
telem.tempCelsius = cachedTempC;
|
|
telem.biasRmsRadS = statBiasRms;
|
|
telem.recalCount = statRecalCount;
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
telem.leftClicks = statLeftClicks;
|
|
telem.rightClicks = statRightClicks;
|
|
#endif
|
|
cfgTelemetry.write ((uint8_t*)&telem, sizeof(telem));
|
|
cfgTelemetry.notify((uint8_t*)&telem, sizeof(telem));
|
|
}
|
|
#endif
|
|
|
|
// ─── Advertising ─────────────────────────────────────────────────────────────
|
|
void startAdvertising() {
|
|
Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
|
|
Bluefruit.Advertising.addTxPower();
|
|
Bluefruit.Advertising.addAppearance(BLE_APPEARANCE_HID_MOUSE);
|
|
Bluefruit.Advertising.addService(blehid);
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
Bluefruit.Advertising.addService(blebas);
|
|
#endif
|
|
Bluefruit.Advertising.addName();
|
|
Bluefruit.Advertising.restartOnDisconnect(true);
|
|
Bluefruit.Advertising.setInterval(32, 244);
|
|
Bluefruit.Advertising.setFastTimeout(30);
|
|
Bluefruit.Advertising.start(0);
|
|
}
|
|
|
|
// ─── Setup ────────────────────────────────────────────────────────────────────
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
unsigned long serialWait = millis();
|
|
while (!Serial && (millis() - serialWait < 2000)) { delay(10); }
|
|
|
|
pinMode(PIN_CHG, INPUT_PULLUP);
|
|
pinMode(LED_RED, OUTPUT); digitalWrite(LED_RED, HIGH);
|
|
pinMode(LED_BLUE, OUTPUT); digitalWrite(LED_BLUE, HIGH);
|
|
|
|
// ── Boot-loop detection ───────────────────────────────────────────────────
|
|
#ifdef FEATURE_BOOT_LOOP_DETECT
|
|
if (bootMagic != 0xCAFEBABE) { bootMagic = 0xCAFEBABE; bootCount = 0; }
|
|
bootCount++;
|
|
Serial.print("[BOOT] count="); Serial.println(bootCount);
|
|
if (bootCount >= 3) {
|
|
bootCount = 0; safeMode = true;
|
|
Serial.println("[BOOT] Boot loop — safe mode (no config service)");
|
|
InternalFS.begin(); InternalFS.remove(CONFIG_FILENAME);
|
|
for (int i=0; i<3; i++) { digitalWrite(LED_RED,LOW); delay(150); digitalWrite(LED_RED,HIGH); delay(150); }
|
|
}
|
|
#endif
|
|
|
|
loadConfig();
|
|
applyChargeMode(cfg.chargeMode);
|
|
|
|
// configAttrTableSize MUST be called before Bluefruit.begin().
|
|
// Value must be >= what HID+DIS+BAS+custom services actually need.
|
|
// Too small crashes just as hard as too large. Floor is 1536.
|
|
Serial.print("[BLE] ATT table: "); Serial.print(ATT_TABLE_SIZE); Serial.println(" bytes");
|
|
Bluefruit.configAttrTableSize(ATT_TABLE_SIZE);
|
|
|
|
Bluefruit.begin(1, 0);
|
|
Bluefruit.setTxPower(4);
|
|
Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
|
|
Bluefruit.Periph.setConnInterval(12, 24); // 15-30ms — less aggressive, prevents stream disconnect
|
|
|
|
Wire1.begin(); // LSM6DS3 is on internal I2C bus (Wire1), must init before imu.begin()
|
|
if (imu.begin() != 0) {
|
|
Serial.println("[ERROR] IMU init failed");
|
|
while(1) { digitalWrite(LED_RED, !digitalRead(LED_RED)); delay(100); }
|
|
}
|
|
Serial.println("[OK] IMU ready");
|
|
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
setupTapDetection();
|
|
#endif
|
|
|
|
cachedTempC = readIMUTemp();
|
|
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
updateBattery();
|
|
#endif
|
|
|
|
calibrateGyroBias();
|
|
|
|
bledis.setManufacturer("Seeed Studio");
|
|
bledis.setModel("XIAO nRF52840 Sense");
|
|
bledis.begin();
|
|
|
|
blehid.begin();
|
|
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
blebas.begin(); blebas.write(100);
|
|
#endif
|
|
|
|
#ifdef FEATURE_CONFIG_SERVICE
|
|
if (!safeMode) {
|
|
setupConfigService();
|
|
Serial.println("[OK] Config service started");
|
|
} else {
|
|
Serial.println("[SAFE] Config service skipped");
|
|
}
|
|
#endif
|
|
|
|
startAdvertising();
|
|
Serial.print("[OK] Advertising — features:");
|
|
#ifdef FEATURE_CONFIG_SERVICE
|
|
Serial.print(" CFG");
|
|
#endif
|
|
#ifdef FEATURE_TELEMETRY
|
|
Serial.print(" TELEM");
|
|
#endif
|
|
#ifdef FEATURE_IMU_STREAM
|
|
Serial.print(" STREAM");
|
|
#endif
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
Serial.print(" TAP");
|
|
#endif
|
|
#ifdef FEATURE_TEMP_COMPENSATION
|
|
Serial.print(" TEMPCOMP");
|
|
#endif
|
|
#ifdef FEATURE_AUTO_RECAL
|
|
Serial.print(" AUTORECAL");
|
|
#endif
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
Serial.print(" BATT");
|
|
#endif
|
|
#ifdef FEATURE_BOOT_LOOP_DETECT
|
|
Serial.print(" BOOTDET");
|
|
#endif
|
|
Serial.println();
|
|
|
|
bootStartMs = millis();
|
|
lastTime = lastBattTime = lastHeartbeat = lastTelemetry = millis();
|
|
}
|
|
|
|
// ─── Loop ─────────────────────────────────────────────────────────────────────
|
|
void loop() {
|
|
unsigned long now = millis();
|
|
|
|
// Clear boot counter after BOOT_SAFE_MS of stable running
|
|
#ifdef FEATURE_BOOT_LOOP_DETECT
|
|
if (!bootCountCleared && (now - bootStartMs >= BOOT_SAFE_MS)) {
|
|
bootCount = 0; bootCountCleared = true;
|
|
Serial.println("[BOOT] Stable — counter cleared");
|
|
}
|
|
#endif
|
|
|
|
if (pendingCal) { pendingCal = false; calibrateGyroBias(); }
|
|
if (pendingReset) { pendingReset = false; factoryReset(); }
|
|
|
|
// Heartbeat LED
|
|
if (now - lastHeartbeat >= HEARTBEAT_MS) {
|
|
lastHeartbeat = now;
|
|
int led = Bluefruit.connected() ? LED_BLUE : LED_RED;
|
|
digitalWrite(led, LOW); delay(HEARTBEAT_DUR); digitalWrite(led, HIGH);
|
|
}
|
|
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
if (now - lastBattTime >= BATT_REPORT_MS) { lastBattTime = now; updateBattery(); }
|
|
#endif
|
|
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
processTaps(now);
|
|
#endif
|
|
|
|
if (now - lastTime < (unsigned long)LOOP_RATE_MS) return;
|
|
float dt = (now - lastTime) / 1000.0f;
|
|
lastTime = now;
|
|
if (dt <= 0.0f || dt > 0.5f) return;
|
|
|
|
cachedTempC = readIMUTemp();
|
|
|
|
#ifdef FEATURE_TELEMETRY
|
|
if (!safeMode && (now - lastTelemetry >= TELEMETRY_MS)) {
|
|
lastTelemetry = now; pushTelemetry(now);
|
|
}
|
|
#endif
|
|
|
|
// Gyro reads with optional temperature compensation
|
|
float gx, gy, gz;
|
|
#ifdef FEATURE_TEMP_COMPENSATION
|
|
float correction = TEMP_COMP_COEFF_DPS_C * (cachedTempC - calTempC);
|
|
gx = (imu.readFloatGyroX() - biasGX - correction) * (PI/180.0f);
|
|
gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f);
|
|
gz = (imu.readFloatGyroZ() - biasGZ - correction) * (PI/180.0f);
|
|
#else
|
|
gx = (imu.readFloatGyroX() - biasGX) * (PI/180.0f);
|
|
gy = (imu.readFloatGyroY() - biasGY) * (PI/180.0f);
|
|
gz = (imu.readFloatGyroZ() - biasGZ) * (PI/180.0f);
|
|
#endif
|
|
|
|
float ax = imu.readFloatAccelX();
|
|
float ay = imu.readFloatAccelY();
|
|
float az = imu.readFloatAccelZ();
|
|
|
|
// Complementary filter
|
|
angleX = ALPHA*(angleX + gx*dt) + (1.0f - ALPHA)*atan2f(ax, sqrtf(ay*ay + az*az));
|
|
angleY = ALPHA*(angleY + gy*dt) + (1.0f - ALPHA)*atan2f(ay, sqrtf(ax*ax + az*az));
|
|
|
|
float fGy = (fabsf(gy) > cfg.deadZone) ? gy : 0.0f;
|
|
float fGz = (fabsf(gz) > cfg.deadZone) ? gz : 0.0f;
|
|
|
|
bool moving = (fGy != 0.0f || fGz != 0.0f);
|
|
if (moving) { idleFrames = 0; idleStartMs = 0; }
|
|
else { idleFrames++; if (idleStartMs == 0) idleStartMs = now; }
|
|
bool idle = (idleFrames >= IDLE_FRAMES);
|
|
|
|
#ifdef FEATURE_AUTO_RECAL
|
|
if (idle && idleStartMs != 0 && (now - idleStartMs >= AUTO_RECAL_MS)) {
|
|
Serial.println("[AUTO-CAL] Long idle — recalibrating...");
|
|
idleStartMs = 0; calibrateGyroBias(); return;
|
|
}
|
|
#endif
|
|
|
|
int8_t moveX = 0, moveY = 0;
|
|
uint8_t flags = 0;
|
|
|
|
if (idle) {
|
|
accumX = accumY = 0.0f;
|
|
flags |= 0x01;
|
|
} else {
|
|
float rawX = applyAcceleration(applyCurve(-fGz * cfg.sensitivity * dt));
|
|
float rawY = applyAcceleration(applyCurve( fGy * cfg.sensitivity * dt));
|
|
if (cfg.axisFlip & 0x01) rawX = -rawX;
|
|
if (cfg.axisFlip & 0x02) rawY = -rawY;
|
|
accumX += rawX; accumY += rawY;
|
|
moveX = (int8_t)constrain((int)accumX, -127, 127);
|
|
moveY = (int8_t)constrain((int)accumY, -127, 127);
|
|
accumX -= moveX; accumY -= moveY;
|
|
if (Bluefruit.connected() && (moveX != 0 || moveY != 0)) blehid.mouseMove(moveX, moveY);
|
|
}
|
|
|
|
#ifdef FEATURE_IMU_STREAM
|
|
if (!safeMode && imuStreamEnabled && Bluefruit.connected()
|
|
&& (now - lastImuStream >= IMU_STREAM_RATE_MS)) {
|
|
lastImuStream = now;
|
|
ImuPacket pkt;
|
|
pkt.gyroY_mDPS = (int16_t)constrain(gy*(180.f/PI)*1000.f, -32000, 32000);
|
|
pkt.gyroZ_mDPS = (int16_t)constrain(gz*(180.f/PI)*1000.f, -32000, 32000);
|
|
pkt.accelX_mg = (int16_t)constrain(ax*1000.f, -32000, 32000);
|
|
pkt.accelY_mg = (int16_t)constrain(ay*1000.f, -32000, 32000);
|
|
pkt.accelZ_mg = (int16_t)constrain(az*1000.f, -32000, 32000);
|
|
pkt.moveX = moveX;
|
|
pkt.moveY = moveY;
|
|
pkt.flags = flags;
|
|
pkt._pad = 0;
|
|
cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt));
|
|
}
|
|
#endif
|
|
|
|
#ifdef DEBUG
|
|
Serial.print("T="); Serial.print(cachedTempC,1);
|
|
Serial.print(" gy="); Serial.print(gy,3);
|
|
Serial.print(" gz="); Serial.print(gz,3);
|
|
Serial.print(" mx="); Serial.print(moveX);
|
|
Serial.print(" my="); Serial.println(moveY);
|
|
#endif
|
|
}
|