Remove unnecessary comments, clean up code
This commit is contained in:
@@ -1,27 +1,12 @@
|
||||
/*
|
||||
* IMU BLE Mouse — Seeed XIAO nRF52840 Sense (v3.4)
|
||||
* 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, ~10 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
|
||||
@@ -40,24 +25,24 @@
|
||||
#include "Wire.h"
|
||||
#include "sleep.h"
|
||||
|
||||
// ─── Boot-loop detection ──────────────────────────────────────────────────────
|
||||
// Boot-loop detection
|
||||
#ifdef FEATURE_BOOT_LOOP_DETECT
|
||||
static uint32_t __attribute__((section(".noinit"))) bootCount;
|
||||
static uint32_t __attribute__((section(".noinit"))) bootMagic;
|
||||
#endif
|
||||
|
||||
// ─── BLE Standard Services ────────────────────────────────────────────────────
|
||||
// BLE Standard Services
|
||||
BLEDis bledis;
|
||||
BLEHidAdafruit blehid;
|
||||
#ifdef FEATURE_BATTERY_MONITOR
|
||||
BLEBas blebas;
|
||||
#endif
|
||||
|
||||
// ─── Persistence ──────────────────────────────────────────────────────────────
|
||||
// Persistence
|
||||
using namespace Adafruit_LittleFS_Namespace;
|
||||
File cfgFile(InternalFS);
|
||||
|
||||
// ─── Config definitions ───────────────────────────────────────────────────────
|
||||
// Config definitions
|
||||
Config cfg;
|
||||
const Config CFG_DEFAULTS = {
|
||||
CONFIG_MAGIC, 600.0f, 0.060f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW,
|
||||
@@ -66,12 +51,12 @@ const Config CFG_DEFAULTS = {
|
||||
/*btnLeftPin=*/BTN_PIN_NONE, /*btnRightPin=*/BTN_PIN_NONE, /*btnMiddlePin=*/BTN_PIN_NONE
|
||||
};
|
||||
|
||||
// ─── Telemetry definition ─────────────────────────────────────────────────────
|
||||
// Telemetry definition
|
||||
#ifdef FEATURE_TELEMETRY
|
||||
TelemetryPacket telem = {};
|
||||
#endif
|
||||
|
||||
// ─── Tuning constants ─────────────────────────────────────────────────────────
|
||||
// Tuning constants
|
||||
const float ALPHA = 0.96f;
|
||||
const int LOOP_RATE_MS = 10;
|
||||
const int BIAS_SAMPLES = 200;
|
||||
@@ -97,7 +82,7 @@ const float BATT_CRITICAL = 3.10f;
|
||||
const unsigned long AUTO_RECAL_MS = 5UL * 60UL * 1000UL;
|
||||
#endif
|
||||
|
||||
// ─── Global state definitions ─────────────────────────────────────────────────
|
||||
// Global state definitions
|
||||
float angleX = 0, angleY = 0;
|
||||
float accumX = 0, accumY = 0;
|
||||
// Low-pass filtered gravity estimate in device frame (for roll-independent axis projection)
|
||||
@@ -132,10 +117,7 @@ uint32_t loopStalls = 0; // loop iterations where dt > 20ms (behind sch
|
||||
bool pendingCal = false;
|
||||
bool pendingReset = false;
|
||||
|
||||
// ── Jerk-based shock detection — freeze cursor during tap impacts ────────────
|
||||
// Jerk = da/dt (rate of change of acceleration). Normal mouse rotation produces
|
||||
// smooth accel changes (low jerk); a tap is a sharp impulse (very high jerk).
|
||||
// This cleanly separates taps from any intentional motion regardless of speed.
|
||||
// Jerk-based shock detection - freeze cursor during tap impacts, doesn't work well yet!
|
||||
unsigned long shockFreezeUntil = 0;
|
||||
float prevAx = 0, prevAy = 0, prevAz = 0; // previous frame's accel for Δa
|
||||
const unsigned long SHOCK_FREEZE_MS = 80; // hold freeze after last spike
|
||||
@@ -161,7 +143,7 @@ unsigned long bootStartMs = 0;
|
||||
bool safeMode = false;
|
||||
bool bootCountCleared = false;
|
||||
|
||||
// ─── Advertising ─────────────────────────────────────────────────────────────
|
||||
// Advertising
|
||||
static void startAdvertising() {
|
||||
Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
|
||||
Bluefruit.Advertising.addTxPower();
|
||||
@@ -177,7 +159,7 @@ static void startAdvertising() {
|
||||
Bluefruit.Advertising.start(0);
|
||||
}
|
||||
|
||||
// ─── Setup ────────────────────────────────────────────────────────────────────
|
||||
// Setup
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
unsigned long serialWait = millis();
|
||||
@@ -188,14 +170,14 @@ void setup() {
|
||||
pinMode(LED_GREEN, OUTPUT); digitalWrite(LED_GREEN, HIGH);
|
||||
pinMode(LED_BLUE, OUTPUT); digitalWrite(LED_BLUE, HIGH);
|
||||
|
||||
// ── Boot-loop detection ───────────────────────────────────────────────────
|
||||
// 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)");
|
||||
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); } // fault: red
|
||||
}
|
||||
@@ -214,7 +196,7 @@ void setup() {
|
||||
Bluefruit.begin(1, 0);
|
||||
Bluefruit.setTxPower(4);
|
||||
Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
|
||||
Bluefruit.Periph.setConnInterval(16, 32); // 20-40ms — wider interval reduces SoftDevice TX stalls
|
||||
Bluefruit.Periph.setConnInterval(16, 32); // 20-40ms - wider interval reduces SoftDevice TX stalls
|
||||
|
||||
Wire1.begin(); // LSM6DS3 is on internal I2C bus (Wire1), must init before imu.begin()
|
||||
if (imu.begin() != 0) {
|
||||
@@ -242,8 +224,6 @@ void setup() {
|
||||
// Seed previous-accel for jerk detection so first frame doesn't spike
|
||||
prevAx = imu.readFloatAccelX(); prevAy = imu.readFloatAccelY(); prevAz = imu.readFloatAccelZ();
|
||||
|
||||
// Sleep manager init: must come after calibrateGyroBias() and imu.begin().
|
||||
// Unconditional — sleep.h is always included, no feature flag needed.
|
||||
sleepManagerInit();
|
||||
|
||||
bledis.setManufacturer("Seeed Studio");
|
||||
@@ -266,7 +246,7 @@ void setup() {
|
||||
#endif
|
||||
|
||||
startAdvertising();
|
||||
Serial.print("[OK] Advertising — features:");
|
||||
Serial.print("[OK] Advertising - features:");
|
||||
#ifdef FEATURE_CONFIG_SERVICE
|
||||
Serial.print(" CFG");
|
||||
#endif
|
||||
@@ -301,7 +281,7 @@ void setup() {
|
||||
lastTime = lastBattTime = lastHeartbeat = lastTelemetry = millis();
|
||||
}
|
||||
|
||||
// ─── Loop ─────────────────────────────────────────────────────────────────────
|
||||
// Loop
|
||||
void loop() {
|
||||
unsigned long now = millis();
|
||||
|
||||
@@ -309,7 +289,7 @@ void loop() {
|
||||
#ifdef FEATURE_BOOT_LOOP_DETECT
|
||||
if (!bootCountCleared && (now - bootStartMs >= BOOT_SAFE_MS)) {
|
||||
bootCount = 0; bootCountCleared = true;
|
||||
Serial.println("[BOOT] Stable — counter cleared");
|
||||
Serial.println("[BOOT] Stable - counter cleared");
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -341,11 +321,6 @@ void loop() {
|
||||
#ifdef FEATURE_PHYSICAL_BUTTONS
|
||||
processPhysicalButtons();
|
||||
#endif
|
||||
|
||||
// Sleep manager runs every iteration — must not be gated by LOOP_RATE_MS
|
||||
// because it needs to catch the imuWakeFlag set by the ISR promptly.
|
||||
// Pass idle=false when in IMU_LP (idleFrames is stale since gyro reads
|
||||
// are skipped); the manager tracks its own idle timestamp internally.
|
||||
{
|
||||
bool idle_for_sleep = (sleepStage == SLEEP_IMU_LP) ? true
|
||||
: (idleFrames >= IDLE_FRAMES);
|
||||
@@ -369,8 +344,6 @@ void loop() {
|
||||
#endif
|
||||
|
||||
// Gyro reads with optional temperature compensation.
|
||||
// (sleepManagerUpdate above already returns early when in IMU_LP/DEEP)
|
||||
|
||||
float correction = 0.0f;
|
||||
#ifdef FEATURE_TEMP_COMPENSATION
|
||||
if (cfg.featureFlags & FLAG_TEMP_COMP_ENABLED)
|
||||
@@ -384,17 +357,14 @@ void loop() {
|
||||
float ay = imu.readFloatAccelY();
|
||||
float az = imu.readFloatAccelZ();
|
||||
|
||||
// ── Jerk-based shock detection — freeze cursor during tap impacts ────────
|
||||
// Jerk = da/dt. Normal rotation = smooth accel changes (low jerk);
|
||||
// a tap is a sharp impulse (very high jerk).
|
||||
// Jerk-based shock detection - freeze cursor during tap impacts, doesn't work well yet
|
||||
float jx = (ax - prevAx) / dt, jy = (ay - prevAy) / dt, jz = (az - prevAz) / dt;
|
||||
float jerkSq = jx*jx + jy*jy + jz*jz;
|
||||
prevAx = ax; prevAy = ay; prevAz = az;
|
||||
bool shocked = cfg.tapFreezeEnabled && ((jerkSq > cfg.jerkThreshold) || (now < shockFreezeUntil));
|
||||
if (cfg.tapFreezeEnabled && jerkSq > cfg.jerkThreshold) shockFreezeUntil = now + SHOCK_FREEZE_MS;
|
||||
|
||||
// Complementary filter — gx=pitch axis, gz=yaw axis on this board layout
|
||||
// During shock: gyro-only integration to avoid accel spike corrupting angles
|
||||
// Complementary filter
|
||||
if (shocked) {
|
||||
angleX += gx * dt;
|
||||
angleY += gz * dt;
|
||||
@@ -403,12 +373,7 @@ void loop() {
|
||||
angleY = ALPHA*(angleY + gz*dt) + (1.0f - ALPHA)*atan2f(ay, sqrtf(ax*ax + az*az));
|
||||
}
|
||||
|
||||
// ── Gravity-based axis decomposition ──────────────────────────────────────
|
||||
// Low-pass filter accel to get a stable gravity estimate in device frame.
|
||||
// This lets us project angular velocity onto world-aligned axes regardless
|
||||
// of how the device is rolled. Device forward (pointing) axis = X.
|
||||
// Confirmed by diagnostics: GX=roll, GY=nod, GZ=pan in user's hold.
|
||||
// Skip update during shock to protect the gravity estimate from tap spikes.
|
||||
// Gravity-based axis decomposition
|
||||
const float GRAV_LP = 0.05f;
|
||||
if (!shocked) {
|
||||
gravX += GRAV_LP * (ax - gravX);
|
||||
@@ -420,8 +385,6 @@ void loop() {
|
||||
if (gN < 0.3f) gN = 1.0f;
|
||||
float gnx = gravX/gN, gny = gravY/gN, gnz = gravZ/gN;
|
||||
|
||||
// Screen-right = cross(forward, up) = cross([1,0,0], [gnx,gny,gnz])
|
||||
// = [0, -gnz, gny]
|
||||
float ry = -gnz, rz = gny;
|
||||
float rN = sqrtf(ry*ry + rz*rz);
|
||||
if (rN < 0.01f) { ry = -1.0f; rz = 0.0f; rN = 1.0f; }
|
||||
@@ -433,11 +396,9 @@ void loop() {
|
||||
float pitchRate = -(gy*ry + gz*rz);
|
||||
|
||||
// Projected rates amplify residual gyro bias (especially GY drift on pitch axis).
|
||||
// Use a wider dead zone for pitch to prevent constant cursor drift at rest.
|
||||
float fYaw = (fabsf(yawRate) > cfg.deadZone) ? yawRate : 0.0f;
|
||||
float fPitch = (fabsf(pitchRate) > cfg.deadZone * 3.0f) ? pitchRate : 0.0f;
|
||||
|
||||
// DIAG: print every 500ms to debug gravity projection — remove when confirmed
|
||||
#ifdef DEBUG
|
||||
{ static unsigned long lastDiag = 0;
|
||||
if (now - lastDiag >= 500) { lastDiag = now;
|
||||
@@ -456,18 +417,16 @@ void loop() {
|
||||
|
||||
#ifdef FEATURE_AUTO_RECAL
|
||||
if ((cfg.featureFlags & FLAG_AUTO_RECAL_ENABLED) && idle && idleStartMs != 0 && (now - idleStartMs >= AUTO_RECAL_MS)) {
|
||||
Serial.println("[AUTO-CAL] Long idle — recalibrating...");
|
||||
Serial.println("[AUTO-CAL] Long idle - recalibrating...");
|
||||
idleStartMs = 0; calibrateGyroBias(); prevAx = imu.readFloatAccelX(); prevAy = imu.readFloatAccelY(); prevAz = imu.readFloatAccelZ(); return;
|
||||
}
|
||||
#endif
|
||||
|
||||
// (sleep manager already called above, before LOOP_RATE_MS gate)
|
||||
|
||||
int8_t moveX = 0, moveY = 0;
|
||||
uint8_t flags = 0;
|
||||
|
||||
if (shocked) {
|
||||
// Shock freeze — discard accumulated sub-pixel motion and suppress output
|
||||
// Shock freeze - discard accumulated sub-pixel motion and suppress output
|
||||
accumX = accumY = 0.0f;
|
||||
flags |= 0x08; // bit3 = shock freeze active
|
||||
} else if (idle) {
|
||||
@@ -491,7 +450,7 @@ void loop() {
|
||||
lastImuStream = now;
|
||||
|
||||
if (now < streamBackoffUntil) {
|
||||
// Backing off — host TX buffer congested, skip to avoid 100ms block
|
||||
// Backing off - host TX buffer congested, skip to avoid 100ms block
|
||||
} else {
|
||||
ImuPacket pkt;
|
||||
pkt.gyroX_mDPS = (int16_t)constrain(gx*(180.f/PI)*1000.f, -32000, 32000);
|
||||
@@ -512,7 +471,7 @@ void loop() {
|
||||
if (streamConsecFails >= STREAM_BACKOFF_THRESH) {
|
||||
streamBackoffUntil = now + STREAM_BACKOFF_MS;
|
||||
streamConsecFails = 0;
|
||||
Serial.print("[STREAM] TX congested — backing off ");
|
||||
Serial.print("[STREAM] TX congested - backing off ");
|
||||
Serial.print(STREAM_BACKOFF_MS); Serial.println("ms");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user