WebUI, telemetry
This commit is contained in:
+464
-381
@@ -1,32 +1,51 @@
|
||||
/*
|
||||
* IMU BLE Mouse — Seeed XIAO nRF52840 Sense (v2 — Full Featured)
|
||||
* IMU BLE Mouse — Seeed XIAO nRF52840 Sense (v3.3)
|
||||
* ================================================================
|
||||
* 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)
|
||||
* Changes vs v3.2:
|
||||
* - 10 characteristics → 4 (fixes SoftDevice RAM overflow)
|
||||
* - configAttrTableSize reduced to 1024
|
||||
* - All config params merged into one 16-byte ConfigBlob (0x1235)
|
||||
* - chargeStatus merged into TelemetryPacket._pad (0x1237)
|
||||
* - ImuStream (0x1238) and Command (0x1236) unchanged
|
||||
* - Boot-loop detection retained
|
||||
*
|
||||
* Required Libraries:
|
||||
* - Seeed Arduino LSM6DS3
|
||||
* - Adafruit nRF52 BSP
|
||||
* ── BLE Config Service (UUID 0x1234) ───────────────────────────────
|
||||
* UUID | Len | R/W/N | Description
|
||||
* ───────|─────|───────|──────────────────────────────────────────
|
||||
* 0x1235 | 16 | R/W | ConfigBlob — all settings in one write
|
||||
* 0x1236 | 1 | W | Command: 0x01=Cal 0xFF=FactoryReset
|
||||
* 0x1237 | 24 | R/N | TelemetryPacket, notified 1 Hz
|
||||
* 0x1238 | 14 | N | ImuPacket, notified ~100 Hz
|
||||
*
|
||||
* 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
|
||||
* ── ConfigBlob (16 bytes, little-endian) ────────────────────────────
|
||||
* float sensitivity [0]
|
||||
* float deadZone [4]
|
||||
* float accelStrength [8]
|
||||
* uint8_t curve [12] 0=LINEAR 1=SQUARE 2=SQRT
|
||||
* uint8_t axisFlip [13] bit0=flipX bit1=flipY
|
||||
* uint8_t chargeMode [14] 0=OFF 1=50mA 2=100mA
|
||||
* uint8_t _pad [15]
|
||||
*
|
||||
* ── 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
|
||||
* ── TelemetryPacket (24 bytes, little-endian) ────────────────────────
|
||||
* 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] 0=discharging 1=charging 2=full
|
||||
* uint8_t _pad [23]
|
||||
*
|
||||
* ── ImuPacket (14 bytes, little-endian) ─────────────────────────────
|
||||
* 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]
|
||||
*/
|
||||
|
||||
#include <bluefruit.h>
|
||||
@@ -38,333 +57,396 @@
|
||||
// ─── Debug ────────────────────────────────────────────────────────────────────
|
||||
// #define DEBUG
|
||||
|
||||
// ─── Boot-loop detection ──────────────────────────────────────────────────────
|
||||
static uint32_t __attribute__((section(".noinit"))) bootCount;
|
||||
static uint32_t __attribute__((section(".noinit"))) bootMagic;
|
||||
static bool safeMode = false;
|
||||
static bool bootCountCleared = false;
|
||||
|
||||
// ─── 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);
|
||||
// ─── BLE Config Service — 4 characteristics only ─────────────────────────────
|
||||
BLEService cfgService (0x1234);
|
||||
BLECharacteristic cfgBlob (0x1235); // ConfigBlob R/W 16 bytes
|
||||
BLECharacteristic cfgCommand (0x1236); // Command W 1 byte
|
||||
BLECharacteristic cfgTelemetry (0x1237); // Telemetry R/N 24 bytes 1 Hz
|
||||
BLECharacteristic cfgImuStream (0x1238); // ImuStream N 14 bytes ~100 Hz
|
||||
|
||||
// ─── IMU ──────────────────────────────────────────────────────────────────────
|
||||
LSM6DS3 imu(I2C_MODE, 0x6A);
|
||||
|
||||
// ─── Pin Definitions ──────────────────────────────────────────────────────────
|
||||
#define LSM6DS3_CTRL1_XL 0x10
|
||||
#define LSM6DS3_TAP_CFG 0x58
|
||||
#define LSM6DS3_TAP_THS_6D 0x59
|
||||
#define LSM6DS3_INT_DUR2 0x5A
|
||||
#define LSM6DS3_WAKE_UP_THS 0x5B
|
||||
#define LSM6DS3_MD1_CFG 0x5E
|
||||
#define LSM6DS3_TAP_SRC 0x1C
|
||||
#define LSM6DS3_OUT_TEMP_L 0x20
|
||||
#define LSM6DS3_OUT_TEMP_H 0x21
|
||||
|
||||
// ─── Pins ─────────────────────────────────────────────────────────────────────
|
||||
#define PIN_VBAT_ENABLE (14)
|
||||
#define PIN_VBAT_READ (32)
|
||||
#define PIN_CHG (17)
|
||||
#define PIN_CHG (23)
|
||||
#define PIN_HICHG (22)
|
||||
|
||||
// ─── EEPROM / Persistence ─────────────────────────────────────────────────────
|
||||
#define CONFIG_FILENAME "/imu_mouse_cfg.bin"
|
||||
#define CONFIG_MAGIC 0xDEAD1234UL
|
||||
// ─── Persistence ──────────────────────────────────────────────────────────────
|
||||
#define CONFIG_FILENAME "/imu_mouse_cfg.bin"
|
||||
#define CONFIG_MAGIC 0xDEAD1238UL // bumped — struct layout unchanged but version tag updated
|
||||
|
||||
using namespace Adafruit_LittleFS_Namespace;
|
||||
File cfgFile(InternalFS);
|
||||
|
||||
// ─── Motion Scaling Curves ────────────────────────────────────────────────────
|
||||
enum CurveType : uint8_t {
|
||||
CURVE_LINEAR = 0,
|
||||
CURVE_SQUARE = 1,
|
||||
CURVE_SQRT = 2
|
||||
};
|
||||
// ─── 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 Struct (persisted) ────────────────────────────────────────────────
|
||||
// ─── Config ───────────────────────────────────────────────────────────────────
|
||||
struct Config {
|
||||
uint32_t magic;
|
||||
float sensitivity;
|
||||
float deadZone;
|
||||
float accelStrength;
|
||||
CurveType curve;
|
||||
uint8_t axisFlip; // bit0=flipX, bit1=flipY
|
||||
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 };
|
||||
|
||||
// ─── Default Parameters ───────────────────────────────────────────────────────
|
||||
const Config CFG_DEFAULTS = {
|
||||
CONFIG_MAGIC,
|
||||
600.0f, // sensitivity
|
||||
0.060f, // dead zone
|
||||
0.08f, // accel strength
|
||||
CURVE_LINEAR,
|
||||
0x00 // no flips
|
||||
// ─── ConfigBlob (what goes over BLE — no magic field) ─────────────────────────
|
||||
struct __attribute__((packed)) ConfigBlob {
|
||||
float sensitivity;
|
||||
float deadZone;
|
||||
float accelStrength;
|
||||
uint8_t curve;
|
||||
uint8_t axisFlip;
|
||||
uint8_t chargeMode;
|
||||
uint8_t _pad;
|
||||
};
|
||||
static_assert(sizeof(ConfigBlob) == 16, "ConfigBlob must be 16 bytes");
|
||||
|
||||
// ─── Fixed Parameters ─────────────────────────────────────────────────────────
|
||||
const float ALPHA = 0.96f;
|
||||
const int LOOP_RATE_MS = 10;
|
||||
const int BIAS_SAMPLES = 200;
|
||||
const int IDLE_FRAMES = 150;
|
||||
// ─── TelemetryPacket ──────────────────────────────────────────────────────────
|
||||
struct __attribute__((packed)) TelemetryPacket {
|
||||
uint32_t uptimeSeconds;
|
||||
uint32_t leftClicks;
|
||||
uint32_t rightClicks;
|
||||
float tempCelsius;
|
||||
float biasRmsRadS;
|
||||
uint16_t recalCount;
|
||||
uint8_t chargeStatus; // replaces old _pad — no extra characteristic needed
|
||||
uint8_t _pad;
|
||||
};
|
||||
static_assert(sizeof(TelemetryPacket) == 24, "TelemetryPacket must be 24 bytes");
|
||||
|
||||
// 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;
|
||||
// ─── ImuPacket ────────────────────────────────────────────────────────────────
|
||||
struct __attribute__((packed)) ImuPacket {
|
||||
int16_t gyroY_mDPS;
|
||||
int16_t gyroZ_mDPS;
|
||||
int16_t accelX_mg;
|
||||
int16_t accelY_mg;
|
||||
int16_t accelZ_mg;
|
||||
int8_t moveX;
|
||||
int8_t moveY;
|
||||
uint8_t flags;
|
||||
uint8_t _pad;
|
||||
};
|
||||
static_assert(sizeof(ImuPacket) == 14, "ImuPacket must be 14 bytes");
|
||||
|
||||
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;
|
||||
// ─── Tuning constants ─────────────────────────────────────────────────────────
|
||||
const float ALPHA = 0.96f;
|
||||
const int LOOP_RATE_MS = 10;
|
||||
const int BIAS_SAMPLES = 200;
|
||||
const int IDLE_FRAMES = 150;
|
||||
const float TEMP_COMP_COEFF_DPS_C = 0.004f;
|
||||
const unsigned long AUTO_RECAL_MS = 5UL * 60UL * 1000UL;
|
||||
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 CLICK_HOLD_MS = 40;
|
||||
const unsigned long DOUBLE_TAP_WINDOW_MS = 400;
|
||||
const unsigned long BOOT_SAFE_MS = 5000;
|
||||
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
|
||||
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;
|
||||
|
||||
unsigned long lastTime = 0;
|
||||
unsigned long lastBattTime = 0;
|
||||
unsigned long lastHeartbeat = 0;
|
||||
unsigned long idleStartMs = 0; // when continuous idle began (0 = not idle)
|
||||
TelemetryPacket telem = {};
|
||||
|
||||
// ─── EEPROM Helpers ───────────────────────────────────────────────────────────
|
||||
bool imuStreamEnabled = false;
|
||||
bool tapPending = false;
|
||||
bool clickButtonDown = false;
|
||||
uint8_t clickButton = 0;
|
||||
unsigned long tapSeenMs = 0;
|
||||
unsigned long clickDownMs = 0;
|
||||
|
||||
bool pendingCal = false;
|
||||
bool pendingReset = false;
|
||||
|
||||
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;
|
||||
|
||||
// ─── I2C helpers ──────────────────────────────────────────────────────────────
|
||||
void imuWriteReg(uint8_t reg, uint8_t val) {
|
||||
Wire.beginTransmission(0x6A); Wire.write(reg); Wire.write(val); Wire.endTransmission();
|
||||
}
|
||||
uint8_t imuReadReg(uint8_t reg) {
|
||||
Wire.beginTransmission(0x6A); Wire.write(reg); Wire.endTransmission(false);
|
||||
Wire.requestFrom((uint8_t)0x6A, (uint8_t)1);
|
||||
return Wire.available() ? Wire.read() : 0;
|
||||
}
|
||||
|
||||
// ─── Temperature ──────────────────────────────────────────────────────────────
|
||||
float readIMUTemp() {
|
||||
int16_t raw = (int16_t)((imuReadReg(LSM6DS3_OUT_TEMP_H) << 8) | imuReadReg(LSM6DS3_OUT_TEMP_L));
|
||||
return 25.0f + (float)raw / 256.0f;
|
||||
}
|
||||
|
||||
// ─── Tap detection ────────────────────────────────────────────────────────────
|
||||
void setupTapDetection() {
|
||||
imuWriteReg(LSM6DS3_CTRL1_XL, 0x60);
|
||||
imuWriteReg(LSM6DS3_TAP_CFG, 0x8E);
|
||||
imuWriteReg(LSM6DS3_TAP_THS_6D, 0x08);
|
||||
imuWriteReg(LSM6DS3_INT_DUR2, 0x77);
|
||||
imuWriteReg(LSM6DS3_WAKE_UP_THS, 0x80);
|
||||
imuWriteReg(LSM6DS3_MD1_CFG, 0x48);
|
||||
Serial.println("[TAP] Engine configured — single=LEFT, double=RIGHT");
|
||||
}
|
||||
|
||||
// ─── 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) {
|
||||
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;
|
||||
}
|
||||
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 to flash");
|
||||
} else {
|
||||
Serial.println("[CFG] ERROR: could not open file for write");
|
||||
}
|
||||
if (cfgFile) { cfgFile.write((uint8_t*)&cfg, sizeof(cfg)); cfgFile.close(); Serial.println("[CFG] Saved"); }
|
||||
else { Serial.println("[CFG] ERROR: write failed"); }
|
||||
}
|
||||
|
||||
// Push current config as a ConfigBlob to the BLE characteristic
|
||||
void pushConfigBlob() {
|
||||
ConfigBlob blob;
|
||||
blob.sensitivity = cfg.sensitivity;
|
||||
blob.deadZone = cfg.deadZone;
|
||||
blob.accelStrength = cfg.accelStrength;
|
||||
blob.curve = (uint8_t)cfg.curve;
|
||||
blob.axisFlip = cfg.axisFlip;
|
||||
blob.chargeMode = (uint8_t)cfg.chargeMode;
|
||||
blob._pad = 0;
|
||||
cfgBlob.write((uint8_t*)&blob, sizeof(blob));
|
||||
}
|
||||
|
||||
void factoryReset() {
|
||||
Serial.println("[CFG] Factory reset!");
|
||||
cfg = CFG_DEFAULTS;
|
||||
cfg = CFG_DEFAULTS; saveConfig();
|
||||
applyChargeMode(cfg.chargeMode);
|
||||
if (!safeMode) pushConfigBlob();
|
||||
telem = {};
|
||||
Serial.println("[CFG] Factory reset complete");
|
||||
}
|
||||
|
||||
// ─── BLE callbacks ────────────────────────────────────────────────────────────
|
||||
// Single callback handles the whole config blob
|
||||
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();
|
||||
// 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);
|
||||
Serial.print("[CFG] Blob written — sens="); Serial.print(cfg.sensitivity);
|
||||
Serial.print(" dz="); Serial.print(cfg.deadZone, 3);
|
||||
Serial.print(" curve="); Serial.print(cfg.curve);
|
||||
Serial.print(" chg="); Serial.println(cfg.chargeMode);
|
||||
}
|
||||
|
||||
// ─── 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");
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
// ─── BLE Config Service Setup ─────────────────────────────────────────────────
|
||||
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");
|
||||
}
|
||||
|
||||
// ─── BLE 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);
|
||||
// ConfigBlob — R/W 16 bytes
|
||||
cfgBlob.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
|
||||
cfgBlob.setPermission(SECMODE_OPEN, SECMODE_OPEN);
|
||||
cfgBlob.setFixedLen(sizeof(ConfigBlob));
|
||||
cfgBlob.setWriteCallback(onConfigBlobWrite);
|
||||
cfgBlob.begin();
|
||||
pushConfigBlob();
|
||||
|
||||
// Command — W 1 byte
|
||||
cfgCommand.setProperties(CHR_PROPS_WRITE);
|
||||
cfgCommand.setPermission(SECMODE_OPEN, SECMODE_OPEN);
|
||||
cfgCommand.setFixedLen(1);
|
||||
cfgCommand.setWriteCallback(onCommandWrite);
|
||||
cfgCommand.begin();
|
||||
|
||||
// Telemetry — R/N 24 bytes
|
||||
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));
|
||||
|
||||
// ImuStream — N 14 bytes
|
||||
cfgImuStream.setProperties(CHR_PROPS_NOTIFY);
|
||||
cfgImuStream.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
|
||||
cfgImuStream.setFixedLen(sizeof(ImuPacket));
|
||||
cfgImuStream.setCccdWriteCallback(onImuStreamCccd);
|
||||
cfgImuStream.begin();
|
||||
}
|
||||
|
||||
// ─── Battery ──────────────────────────────────────────────────────────────────
|
||||
float readBatteryVoltage() {
|
||||
pinMode(PIN_VBAT_ENABLE, OUTPUT);
|
||||
digitalWrite(PIN_VBAT_ENABLE, LOW);
|
||||
delay(1);
|
||||
|
||||
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;
|
||||
|
||||
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);
|
||||
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 charging = (digitalRead(PIN_CHG) == LOW);
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
// chargeStatus is now pushed via telemetry packet — no separate characteristic
|
||||
telem.chargeStatus = (uint8_t)status;
|
||||
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); }
|
||||
}
|
||||
|
||||
// ─── Gyro Calibration ─────────────────────────────────────────────────────────
|
||||
// ─── 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);
|
||||
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)(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;
|
||||
|
||||
biasGX = (float)(sx/BIAS_SAMPLES); biasGY = (float)(sy/BIAS_SAMPLES); biasGZ = (float)(sz/BIAS_SAMPLES);
|
||||
calTempC = readIMUTemp();
|
||||
angleX = angleY = accumX = accumY = 0.0f;
|
||||
telem.recalCount++;
|
||||
float bxr = biasGX*(PI/180.f), byr = biasGY*(PI/180.f), bzr = biasGZ*(PI/180.f);
|
||||
telem.biasRmsRadS = sqrtf((bxr*bxr + byr*byr + bzr*bzr) / 3.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);
|
||||
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.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;
|
||||
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); }
|
||||
|
||||
// ─── BLE Advertising ──────────────────────────────────────────────────────────
|
||||
// ─── Tap state machine ────────────────────────────────────────────────────────
|
||||
void processTaps(unsigned long now) {
|
||||
if (clickButtonDown && (now - clickDownMs >= CLICK_HOLD_MS)) {
|
||||
blehid.mouseButtonPress(clickButton, false);
|
||||
clickButtonDown = false; clickButton = 0;
|
||||
}
|
||||
uint8_t tapSrc = imuReadReg(LSM6DS3_TAP_SRC);
|
||||
bool singleTap = (tapSrc & 0x20) != 0;
|
||||
bool doubleTap = (tapSrc & 0x10) != 0;
|
||||
bool tapEvent = (tapSrc & 0x40) != 0;
|
||||
if (!tapEvent) {
|
||||
if (tapPending && (now - tapSeenMs >= DOUBLE_TAP_WINDOW_MS)) {
|
||||
tapPending = false;
|
||||
if (!clickButtonDown) {
|
||||
Serial.println("[TAP] Single → LEFT");
|
||||
blehid.mouseButtonPress(MOUSE_BUTTON_LEFT, true);
|
||||
clickButton = MOUSE_BUTTON_LEFT; clickButtonDown = true; clickDownMs = now;
|
||||
telem.leftClicks++;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (doubleTap && !clickButtonDown) {
|
||||
tapPending = false;
|
||||
Serial.println("[TAP] Double → RIGHT");
|
||||
blehid.mouseButtonPress(MOUSE_BUTTON_RIGHT, true);
|
||||
clickButton = MOUSE_BUTTON_RIGHT; clickButtonDown = true; clickDownMs = now;
|
||||
telem.rightClicks++;
|
||||
return;
|
||||
}
|
||||
if (singleTap && !tapPending && !clickButtonDown) { tapPending = true; tapSeenMs = now; }
|
||||
}
|
||||
|
||||
// ─── Telemetry ────────────────────────────────────────────────────────────────
|
||||
void pushTelemetry(unsigned long now) {
|
||||
telem.uptimeSeconds = now / 1000;
|
||||
telem.tempCelsius = cachedTempC;
|
||||
// telem.chargeStatus is updated in updateBattery()
|
||||
cfgTelemetry.write ((uint8_t*)&telem, sizeof(telem));
|
||||
cfgTelemetry.notify((uint8_t*)&telem, sizeof(telem));
|
||||
}
|
||||
|
||||
// ─── Advertising ─────────────────────────────────────────────────────────────
|
||||
void startAdvertising() {
|
||||
Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
|
||||
Bluefruit.Advertising.addTxPower();
|
||||
@@ -378,169 +460,170 @@ void startAdvertising() {
|
||||
Bluefruit.Advertising.start(0);
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
// ─── Setup ────────────────────────────────────────────────────────────────────
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
while (!Serial) delay(10);
|
||||
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);
|
||||
|
||||
// ── 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); }
|
||||
// ── Boot-loop detection ───────────────────────────────────────────────────
|
||||
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 detected — safe mode");
|
||||
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); }
|
||||
}
|
||||
Serial.println("[OK] IMU initialised");
|
||||
|
||||
loadConfig();
|
||||
applyChargeMode(cfg.chargeMode);
|
||||
|
||||
// 1024 is sufficient for 4 characteristics (was 3072/2048 — both overflowed)
|
||||
Bluefruit.configAttrTableSize(1024);
|
||||
|
||||
Bluefruit.begin(1, 0);
|
||||
Bluefruit.setTxPower(4);
|
||||
Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
|
||||
Bluefruit.Periph.setConnInterval(6, 12);
|
||||
|
||||
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");
|
||||
|
||||
setupTapDetection();
|
||||
cachedTempC = readIMUTemp();
|
||||
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);
|
||||
blebas.begin(); blebas.write(100);
|
||||
|
||||
// Config service must begin AFTER Bluefruit.begin()
|
||||
setupConfigService();
|
||||
if (!safeMode) {
|
||||
setupConfigService();
|
||||
Serial.println("[OK] Config service started (4 characteristics)");
|
||||
} else {
|
||||
Serial.println("[SAFE] Config service skipped — basic mouse only");
|
||||
}
|
||||
|
||||
startAdvertising();
|
||||
Serial.println("[OK] BLE advertising — pair 'IMU Mouse' on your host");
|
||||
Serial.println(" Config service UUID 0x1234 available for tuning");
|
||||
Serial.print("[OK] Advertising as '");
|
||||
Serial.print(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
|
||||
Serial.println("'");
|
||||
|
||||
lastTime = millis();
|
||||
lastBattTime = millis();
|
||||
lastHeartbeat = millis();
|
||||
idleStartMs = 0;
|
||||
bootStartMs = millis();
|
||||
lastTime = lastBattTime = lastHeartbeat = lastTelemetry = millis();
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
// ─── Loop ─────────────────────────────────────────────────────────────────────
|
||||
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();
|
||||
if (!bootCountCleared && (now - bootStartMs >= BOOT_SAFE_MS)) {
|
||||
bootCount = 0; bootCountCleared = true;
|
||||
Serial.println("[BOOT] Stable — boot counter cleared");
|
||||
}
|
||||
|
||||
// ── Heartbeat LED ─────────────────────────────────────────────────────────
|
||||
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);
|
||||
digitalWrite(led, LOW); delay(HEARTBEAT_DUR); digitalWrite(led, HIGH);
|
||||
}
|
||||
|
||||
// ── Battery ───────────────────────────────────────────────────────────────
|
||||
if (now - lastBattTime >= BATT_REPORT_MS) {
|
||||
lastBattTime = now;
|
||||
updateBattery();
|
||||
}
|
||||
if (now - lastBattTime >= BATT_REPORT_MS) { lastBattTime = now; updateBattery(); }
|
||||
|
||||
processTaps(now);
|
||||
|
||||
// ── 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);
|
||||
cachedTempC = readIMUTemp();
|
||||
|
||||
if (!safeMode && (now - lastTelemetry >= TELEMETRY_MS)) {
|
||||
lastTelemetry = now; pushTelemetry(now);
|
||||
}
|
||||
|
||||
float tempDelta = cachedTempC - calTempC;
|
||||
float correction = TEMP_COMP_COEFF_DPS_C * tempDelta;
|
||||
|
||||
float gx = (imu.readFloatGyroX() - biasGX - correction) * (PI/180.0f);
|
||||
float gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f);
|
||||
float gz = (imu.readFloatGyroZ() - biasGZ - correction) * (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));
|
||||
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
|
||||
}
|
||||
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);
|
||||
|
||||
// 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;
|
||||
Serial.println("[AUTO-CAL] Long idle — recalibrating...");
|
||||
idleStartMs = 0; calibrateGyroBias(); return;
|
||||
}
|
||||
|
||||
int8_t moveX = 0, moveY = 0;
|
||||
uint8_t flags = 0;
|
||||
|
||||
if (idle) {
|
||||
accumX = 0.0f;
|
||||
accumY = 0.0f;
|
||||
#ifdef DEBUG
|
||||
Serial.println("[IDLE]");
|
||||
#endif
|
||||
return;
|
||||
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);
|
||||
}
|
||||
|
||||
// ── 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);
|
||||
if (!safeMode && imuStreamEnabled && Bluefruit.connected()) {
|
||||
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));
|
||||
}
|
||||
|
||||
#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);
|
||||
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
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user