commit ebde95a403fe68aa9a4de13367389792728bd3d4 Author: Nik Rozman Date: Sat Feb 28 23:31:25 2026 +0100 Initial commit diff --git a/air-mouse.ino b/air-mouse.ino new file mode 100644 index 0000000..8591aa3 --- /dev/null +++ b/air-mouse.ino @@ -0,0 +1,546 @@ +/* + * IMU BLE Mouse — Seeed XIAO nRF52840 Sense (v2 — Full Featured) + * ================================================================ + * Board BSP : Adafruit nRF52 (NOT Seeed mbed BSP) + * Board manager URL: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json + * Select board: "Seeed XIAO nRF52840 Sense" (listed under Adafruit nRF52) + * + * Required Libraries: + * - Seeed Arduino LSM6DS3 + * - Adafruit nRF52 BSP + * + * New in v2: + * 1. BLE Configuration Service — UUID 0x1234 with writable characteristics + * 2. EEPROM persistence — config saved to flash via InternalFileSystem + * 3. BLE calibration trigger — write 0x01 to CAL characteristic + * 4. Motion scaling curve select — LINEAR / SQUARE / SQRT + * 5. Factory Reset command — write 0xFF to CMD characteristic + * 6. Auto-recalibrate on idle — after AUTO_RECAL_MINUTES minutes of stillness + * 7. Axis flip flags — flip X and/or Y via BLE config + * + * ── BLE Config Service layout (UUID 0x1234) ──────────────────── + * Characteristic | UUID | Len | Description + * ──────────────────|────────|─────|────────────────────────── + * Sensitivity | 0x1235 | 4 | float, cursor speed + * Dead Zone | 0x1236 | 4 | float, noise floor rad/s + * Accel Strength | 0x1237 | 4 | float, pointer accel + * Curve Select | 0x1238 | 1 | 0=LINEAR 1=SQUARE 2=SQRT + * Axis Flip | 0x1239 | 1 | bit0=flipX bit1=flipY + * Command | 0x123A | 1 | 0x01=Calibrate 0xFF=FactoryReset + */ + +#include +#include +#include +#include "LSM6DS3.h" +#include "Wire.h" + +// ─── Debug ──────────────────────────────────────────────────────────────────── +// #define DEBUG + +// ─── BLE Standard Services ──────────────────────────────────────────────────── +BLEDis bledis; +BLEHidAdafruit blehid; +BLEBas blebas; + +// ─── BLE Config Service & Characteristics ──────────────────────────────────── +BLEService cfgService(0x1234); +BLECharacteristic cfgSensitivity (0x1235); +BLECharacteristic cfgDeadZone (0x1236); +BLECharacteristic cfgAccelStr (0x1237); +BLECharacteristic cfgCurve (0x1238); +BLECharacteristic cfgAxisFlip (0x1239); +BLECharacteristic cfgCommand (0x123A); + +// ─── IMU ────────────────────────────────────────────────────────────────────── +LSM6DS3 imu(I2C_MODE, 0x6A); + +// ─── Pin Definitions ────────────────────────────────────────────────────────── +#define PIN_VBAT_ENABLE (14) +#define PIN_VBAT_READ (32) +#define PIN_CHG (17) + +// ─── EEPROM / Persistence ───────────────────────────────────────────────────── +#define CONFIG_FILENAME "/imu_mouse_cfg.bin" +#define CONFIG_MAGIC 0xDEAD1234UL + +using namespace Adafruit_LittleFS_Namespace; +File cfgFile(InternalFS); + +// ─── Motion Scaling Curves ──────────────────────────────────────────────────── +enum CurveType : uint8_t { + CURVE_LINEAR = 0, + CURVE_SQUARE = 1, + CURVE_SQRT = 2 +}; + +// ─── Config Struct (persisted) ──────────────────────────────────────────────── +struct Config { + uint32_t magic; + float sensitivity; + float deadZone; + float accelStrength; + CurveType curve; + uint8_t axisFlip; // bit0=flipX, bit1=flipY +}; + +Config cfg; + +// ─── Default Parameters ─────────────────────────────────────────────────────── +const Config CFG_DEFAULTS = { + CONFIG_MAGIC, + 600.0f, // sensitivity + 0.060f, // dead zone + 0.08f, // accel strength + CURVE_LINEAR, + 0x00 // no flips +}; + +// ─── Fixed Parameters ───────────────────────────────────────────────────────── +const float ALPHA = 0.96f; +const int LOOP_RATE_MS = 10; +const int BIAS_SAMPLES = 200; +const int IDLE_FRAMES = 150; + +// Auto-recalibrate: recalibrate after this many minutes of continuous idle +const unsigned long AUTO_RECAL_MINUTES = 5; +const unsigned long AUTO_RECAL_MS = AUTO_RECAL_MINUTES * 60UL * 1000UL; + +const unsigned long BATT_REPORT_MS = 10000; +const unsigned long HEARTBEAT_MS = 2000; +const int HEARTBEAT_DUR = 30; + +const float BATT_FULL = 4.20f; +const float BATT_EMPTY = 3.00f; +const float BATT_CRITICAL = 3.10f; + +// ─── State ──────────────────────────────────────────────────────────────────── +float angleX = 0.0f, angleY = 0.0f; +float accumX = 0.0f, accumY = 0.0f; +float biasGX = 0.0f, biasGY = 0.0f, biasGZ = 0.0f; +int idleFrames = 0; +bool pendingCal = false; // set by BLE write callback +bool pendingReset = false; // set by BLE write callback + +unsigned long lastTime = 0; +unsigned long lastBattTime = 0; +unsigned long lastHeartbeat = 0; +unsigned long idleStartMs = 0; // when continuous idle began (0 = not idle) + +// ─── EEPROM Helpers ─────────────────────────────────────────────────────────── +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) { + Serial.println("[CFG] Bad magic — using defaults"); + cfg = CFG_DEFAULTS; + } else { + Serial.println("[CFG] Loaded from flash"); + } + } else { + Serial.println("[CFG] No file — using defaults"); + cfg = CFG_DEFAULTS; + } +} + +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 to flash"); + } else { + Serial.println("[CFG] ERROR: could not open file for write"); + } +} + +void factoryReset() { + Serial.println("[CFG] Factory reset!"); + cfg = CFG_DEFAULTS; + saveConfig(); + // Push defaults back to BLE characteristics + cfgSensitivity.write((uint8_t*)&cfg.sensitivity, 4); + cfgDeadZone.write ((uint8_t*)&cfg.deadZone, 4); + cfgAccelStr.write ((uint8_t*)&cfg.accelStrength, 4); + cfgCurve.write ((uint8_t*)&cfg.curve, 1); + cfgAxisFlip.write ((uint8_t*)&cfg.axisFlip, 1); +} + +// ─── BLE Write Callbacks ────────────────────────────────────────────────────── +void onSensitivityWrite(uint16_t conn_hdl, BLECharacteristic* chr, + uint8_t* data, uint16_t len) { + if (len == 4) { memcpy(&cfg.sensitivity, data, 4); saveConfig(); } +} +void onDeadZoneWrite(uint16_t conn_hdl, BLECharacteristic* chr, + uint8_t* data, uint16_t len) { + if (len == 4) { memcpy(&cfg.deadZone, data, 4); saveConfig(); } +} +void onAccelStrWrite(uint16_t conn_hdl, BLECharacteristic* chr, + uint8_t* data, uint16_t len) { + if (len == 4) { memcpy(&cfg.accelStrength, data, 4); saveConfig(); } +} +void onCurveWrite(uint16_t conn_hdl, BLECharacteristic* chr, + uint8_t* data, uint16_t len) { + if (len == 1 && data[0] <= 2) { + cfg.curve = (CurveType)data[0]; + saveConfig(); + Serial.print("[CFG] Curve -> "); Serial.println(cfg.curve); + } +} +void onAxisFlipWrite(uint16_t conn_hdl, BLECharacteristic* chr, + uint8_t* data, uint16_t len) { + if (len == 1) { + cfg.axisFlip = data[0]; + saveConfig(); + Serial.print("[CFG] AxisFlip -> 0x"); Serial.println(cfg.axisFlip, HEX); + } +} +void onCommandWrite(uint16_t conn_hdl, BLECharacteristic* chr, + uint8_t* data, uint16_t len) { + if (len < 1) return; + if (data[0] == 0x01) { + pendingCal = true; + Serial.println("[CMD] Calibration requested via BLE"); + } else if (data[0] == 0xFF) { + pendingReset = true; + Serial.println("[CMD] Factory reset requested via BLE"); + } +} + +// ─── BLE Config Service Setup ───────────────────────────────────────────────── +void setupConfigService() { + cfgService.begin(); + + // Each characteristic: READ | WRITE, no response needed for writes + auto props = CHR_PROPS_READ | CHR_PROPS_WRITE; + + cfgSensitivity.setProperties(props); + cfgSensitivity.setPermission(SECMODE_OPEN, SECMODE_OPEN); + cfgSensitivity.setFixedLen(4); + cfgSensitivity.setWriteCallback(onSensitivityWrite); + cfgSensitivity.begin(); + cfgSensitivity.write((uint8_t*)&cfg.sensitivity, 4); + + cfgDeadZone.setProperties(props); + cfgDeadZone.setPermission(SECMODE_OPEN, SECMODE_OPEN); + cfgDeadZone.setFixedLen(4); + cfgDeadZone.setWriteCallback(onDeadZoneWrite); + cfgDeadZone.begin(); + cfgDeadZone.write((uint8_t*)&cfg.deadZone, 4); + + cfgAccelStr.setProperties(props); + cfgAccelStr.setPermission(SECMODE_OPEN, SECMODE_OPEN); + cfgAccelStr.setFixedLen(4); + cfgAccelStr.setWriteCallback(onAccelStrWrite); + cfgAccelStr.begin(); + cfgAccelStr.write((uint8_t*)&cfg.accelStrength, 4); + + cfgCurve.setProperties(props); + cfgCurve.setPermission(SECMODE_OPEN, SECMODE_OPEN); + cfgCurve.setFixedLen(1); + cfgCurve.setWriteCallback(onCurveWrite); + cfgCurve.begin(); + cfgCurve.write((uint8_t*)&cfg.curve, 1); + + cfgAxisFlip.setProperties(props); + cfgAxisFlip.setPermission(SECMODE_OPEN, SECMODE_OPEN); + cfgAxisFlip.setFixedLen(1); + cfgAxisFlip.setWriteCallback(onAxisFlipWrite); + cfgAxisFlip.begin(); + cfgAxisFlip.write((uint8_t*)&cfg.axisFlip, 1); + + cfgCommand.setProperties(CHR_PROPS_WRITE); + cfgCommand.setPermission(SECMODE_OPEN, SECMODE_OPEN); + cfgCommand.setFixedLen(1); + cfgCommand.setWriteCallback(onCommandWrite); + cfgCommand.begin(); +} + +// ─── Battery ────────────────────────────────────────────────────────────────── +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); + + float v = (raw / 4096.0f) * 3.0f * 2.0f; + + Serial.print("[BATT DBG] raw="); Serial.print(raw); + Serial.print(" ("); Serial.print(v, 3); Serial.print("V)"); + Serial.print(" CHG pin="); Serial.println(digitalRead(PIN_CHG)); + return v; +} + +int batteryPercent(float v) { + return (int) constrain((v - BATT_EMPTY) / (BATT_FULL - BATT_EMPTY) * 100.0f, 0, 100); +} + +void updateBattery() { + float v = readBatteryVoltage(); + int pct = batteryPercent(v); + bool charging = (digitalRead(PIN_CHG) == LOW); + + blebas.write(pct); + + Serial.print("[BATT] "); + Serial.print(v, 2); Serial.print("V "); + Serial.print(pct); Serial.print("%"); + if (charging) Serial.print(" [CHARGING]"); + else if (pct >= 99) Serial.print(" [FULL]"); + else if (v < BATT_CRITICAL) Serial.print(" [CRITICAL - CHARGE NOW]"); + else Serial.print(" [ON BATTERY]"); + Serial.println(); + + if (!charging && v < BATT_CRITICAL) { + pinMode(LED_RED, OUTPUT); + for (int i = 0; i < 6; i++) { + digitalWrite(LED_RED, LOW); delay(80); + digitalWrite(LED_RED, HIGH); delay(80); + } + } +} + +// ─── Gyro Calibration ───────────────────────────────────────────────────────── +void calibrateGyroBias() { + Serial.println("[CAL] Hold still — calibrating gyro bias..."); + pinMode(LED_BLUE, OUTPUT); + + double sumX = 0, sumY = 0, sumZ = 0; + for (int i = 0; i < BIAS_SAMPLES; i++) { + sumX += imu.readFloatGyroX(); + sumY += imu.readFloatGyroY(); + sumZ += imu.readFloatGyroZ(); + digitalWrite(LED_BLUE, (i % 20 < 10)); + delay(5); + } + + biasGX = (float)(sumX / BIAS_SAMPLES); + biasGY = (float)(sumY / BIAS_SAMPLES); + biasGZ = (float)(sumZ / BIAS_SAMPLES); + + // Reset angle state to avoid a jump after recal + angleX = 0.0f; + angleY = 0.0f; + accumX = 0.0f; + accumY = 0.0f; + + digitalWrite(LED_BLUE, HIGH); + Serial.print("[CAL] Done. Bias — gx:"); Serial.print(biasGX, 4); + Serial.print(" gy:"); Serial.print(biasGY, 4); + Serial.print(" gz:"); Serial.println(biasGZ, 4); +} + +// ─── Motion Scaling ─────────────────────────────────────────────────────────── +float applyAcceleration(float delta) { + // Pointer acceleration on top of curve + return delta * (1.0f + fabsf(delta) * cfg.accelStrength); +} + +float applyCurve(float v) { + switch (cfg.curve) { + case CURVE_SQUARE: + return (v >= 0.0f ? 1.0f : -1.0f) * v * v; + case CURVE_SQRT: + return (v >= 0.0f ? 1.0f : -1.0f) * sqrtf(fabsf(v)); + case CURVE_LINEAR: + default: + return v; + } +} + +// ─── BLE 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); + Bluefruit.Advertising.addService(blebas); + Bluefruit.Advertising.addName(); + Bluefruit.Advertising.restartOnDisconnect(true); + Bluefruit.Advertising.setInterval(32, 244); + Bluefruit.Advertising.setFastTimeout(30); + Bluefruit.Advertising.start(0); +} + +// ───────────────────────────────────────────────────────────────────────────── +void setup() { + Serial.begin(115200); + while (!Serial) delay(10); + + pinMode(PIN_CHG, INPUT_PULLUP); + pinMode(LED_RED, OUTPUT); digitalWrite(LED_RED, HIGH); + pinMode(LED_BLUE, OUTPUT); digitalWrite(LED_BLUE, HIGH); + + // ── Load persisted config ───────────────────────────────────────────────── + loadConfig(); + + // ── IMU ─────────────────────────────────────────────────────────────────── + if (imu.begin() != 0) { + Serial.println("[ERROR] IMU init failed."); + while (1) { digitalWrite(LED_RED, !digitalRead(LED_RED)); delay(100); } + } + Serial.println("[OK] IMU initialised"); + + updateBattery(); + calibrateGyroBias(); + + // ── BLE ─────────────────────────────────────────────────────────────────── + Bluefruit.begin(); + Bluefruit.setTxPower(4); + Bluefruit.setName("IMU Mouse"); + Bluefruit.Periph.setConnInterval(6, 12); + + bledis.setManufacturer("Seeed Studio"); + bledis.setModel("XIAO nRF52840 Sense"); + bledis.begin(); + + blehid.begin(); + blebas.begin(); + blebas.write(100); + + // Config service must begin AFTER Bluefruit.begin() + setupConfigService(); + + startAdvertising(); + Serial.println("[OK] BLE advertising — pair 'IMU Mouse' on your host"); + Serial.println(" Config service UUID 0x1234 available for tuning"); + + lastTime = millis(); + lastBattTime = millis(); + lastHeartbeat = millis(); + idleStartMs = 0; +} + +// ───────────────────────────────────────────────────────────────────────────── +void loop() { + unsigned long now = millis(); + + // ── Deferred commands (from BLE callbacks, safe to run on main thread) ──── + 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); + } + + // ── Battery ─────────────────────────────────────────────────────────────── + if (now - lastBattTime >= BATT_REPORT_MS) { + lastBattTime = now; + updateBattery(); + } + + // ── IMU rate limit ──────────────────────────────────────────────────────── + 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; + + // ── Read IMU ────────────────────────────────────────────────────────────── + float gx = (imu.readFloatGyroX() - biasGX) * (PI / 180.0f); + float gy = (imu.readFloatGyroY() - biasGY) * (PI / 180.0f); + float gz = (imu.readFloatGyroZ() - biasGZ) * (PI / 180.0f); + + 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)); + + // ── Dead zone ───────────────────────────────────────────────────────────── + float filteredGy = (fabsf(gy) > cfg.deadZone) ? gy : 0.0f; + float filteredGz = (fabsf(gz) > cfg.deadZone) ? gz : 0.0f; + + // ── Idle detection + auto-recalibrate ───────────────────────────────────── + bool moving = (filteredGy != 0.0f || filteredGz != 0.0f); + if (moving) { + idleFrames = 0; + idleStartMs = 0; + } else { + idleFrames++; + if (idleStartMs == 0) idleStartMs = now; // mark start of idle streak + } + + bool idle = (idleFrames >= IDLE_FRAMES); + + // Auto-recalibrate after AUTO_RECAL_MS of continuous stillness + if (idle && idleStartMs != 0 && (now - idleStartMs >= AUTO_RECAL_MS)) { + Serial.println("[AUTO-CAL] Long idle detected — recalibrating..."); + idleStartMs = 0; // reset so we don't retrigger immediately + calibrateGyroBias(); + return; + } + + if (idle) { + accumX = 0.0f; + accumY = 0.0f; +#ifdef DEBUG + Serial.println("[IDLE]"); +#endif + return; + } + + // ── Delta + curve + acceleration + sub-pixel accumulation ───────────────── + float rawX = -filteredGz * cfg.sensitivity * dt; + float rawY = filteredGy * cfg.sensitivity * dt; + + rawX = applyCurve(rawX); + rawY = applyCurve(rawY); + + rawX = applyAcceleration(rawX); + rawY = applyAcceleration(rawY); + + // ── Axis flip ───────────────────────────────────────────────────────────── + if (cfg.axisFlip & 0x01) rawX = -rawX; // flip X + if (cfg.axisFlip & 0x02) rawY = -rawY; // flip Y + + accumX += rawX; + accumY += rawY; + + int8_t moveX = (int8_t) constrain((int)accumX, -127, 127); + int8_t moveY = (int8_t) constrain((int)accumY, -127, 127); + + accumX -= moveX; + accumY -= moveY; + + // ── BLE HID ─────────────────────────────────────────────────────────────── + if (Bluefruit.connected() && (moveX != 0 || moveY != 0)) { + blehid.mouseMove(moveX, moveY); + } + +#ifdef DEBUG + 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 +}