564 lines
21 KiB
C++
564 lines
21 KiB
C++
/*
|
|
* IMU BLE Mouse - Seeed XIAO nRF52840 Sense (v3.4)
|
|
* ================================================================
|
|
* 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
|
|
*
|
|
* Dependencies:
|
|
* FEATURE_TELEMETRY requires FEATURE_CONFIG_SERVICE
|
|
* FEATURE_IMU_STREAM requires FEATURE_CONFIG_SERVICE
|
|
* ================================================================
|
|
*/
|
|
|
|
#include "config.h"
|
|
#include "imu.h"
|
|
#include "motion.h"
|
|
#include "ble_config.h"
|
|
#include "battery.h"
|
|
#include "buttons.h"
|
|
#include <bluefruit.h>
|
|
#include <Adafruit_LittleFS.h>
|
|
#include <InternalFileSystem.h>
|
|
#include "Wire.h"
|
|
#include "sleep.h"
|
|
|
|
// 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
|
|
BLEDis bledis;
|
|
BLEHidAdafruit blehid;
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
BLEBas blebas;
|
|
#endif
|
|
|
|
// Persistence
|
|
using namespace Adafruit_LittleFS_Namespace;
|
|
File cfgFile(InternalFS);
|
|
|
|
// Config definitions
|
|
Config cfg;
|
|
const Config CFG_DEFAULTS = {
|
|
CONFIG_MAGIC, 600.0f, 0.020f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW,
|
|
/*tapThreshold=*/12, /*tapAction=*/TAP_ACTION_KEY, /*tapKey=*/0x04, /*tapMod=*/0x03, // Ctrl+Shift+A
|
|
/*jerkThreshold=*/2000.0f, /*tapFreezeEnabled=*/1, /*featureFlags=*/FLAG_ALL_DEFAULT
|
|
};
|
|
|
|
// Telemetry definition
|
|
#ifdef FEATURE_TELEMETRY
|
|
TelemetryPacket telem = {};
|
|
#endif
|
|
|
|
// Tuning constants
|
|
const int IMU_POLL_MS = 2; // FIFO poll period; samples arrive at 208 Hz
|
|
const int BIAS_SAMPLES = 200;
|
|
const unsigned long IDLE_MS = 1500; // no motion for this long = idle (sleep countdown starts)
|
|
// Requested BLE connection interval (units of 1.25 ms). HID reports are paced to the
|
|
// negotiated interval, so the notify queue never fills and the loop never blocks.
|
|
// Slave latency lets the radio skip empty events while the cursor is still; reports
|
|
// are still sent at the next event, so it adds no pointer latency.
|
|
const uint16_t CONN_INTERVAL_MIN = 6; // 7.5 ms
|
|
const uint16_t CONN_INTERVAL_MAX = 9; // 11.25 ms
|
|
const uint16_t CONN_SLAVE_LATENCY = 4;
|
|
const unsigned long BATT_REPORT_MS = 20000;
|
|
const unsigned long TELEMETRY_MS = 1000;
|
|
const unsigned long HEARTBEAT_MS = 10000;
|
|
const int HEARTBEAT_DUR = 30;
|
|
const unsigned long BOOT_SAFE_MS = 5000;
|
|
#ifdef FEATURE_IMU_STREAM
|
|
const unsigned long IMU_STREAM_RATE_MS = 20;
|
|
#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
|
|
|
|
// Global state definitions
|
|
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;
|
|
uint32_t streamNotifyFails = 0;
|
|
uint32_t streamNotifyOk = 0;
|
|
unsigned long lastStreamDiag = 0;
|
|
// Back-off state: after STREAM_BACKOFF_THRESH consecutive fails, skip notifies
|
|
// for STREAM_BACKOFF_MS to let the SoftDevice HVN TX semaphore drain.
|
|
// Without this, every notify() blocks for BLE_GENERIC_TIMEOUT (100ms).
|
|
uint8_t streamConsecFails = 0;
|
|
unsigned long streamBackoffUntil = 0;
|
|
const uint8_t STREAM_BACKOFF_THRESH = 2; // fails before backing off
|
|
const unsigned long STREAM_BACKOFF_MS = 500; // cooldown window
|
|
#endif
|
|
|
|
bool pendingCal = false;
|
|
bool pendingReset = false;
|
|
#ifdef FEATURE_OTA
|
|
bool pendingOTA = false;
|
|
#endif
|
|
|
|
|
|
ChargeStatus lastChargeStatus = CHGSTAT_DISCHARGING;
|
|
|
|
unsigned long lastMotionMs = 0;
|
|
unsigned long lastImuPoll = 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
|
|
|
|
bool safeMode = false;
|
|
bool bootCountCleared = false;
|
|
|
|
// Connection
|
|
static volatile unsigned long connectedAtMs = 0;
|
|
static volatile bool connParamPending = false;
|
|
static void onConnect(uint16_t connHdl) {
|
|
(void)connHdl;
|
|
connectedAtMs = millis();
|
|
connParamPending = true;
|
|
}
|
|
|
|
// Advertising
|
|
static 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_GREEN, OUTPUT); digitalWrite(LED_GREEN, 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); } // fault: red
|
|
}
|
|
#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.configPrphBandwidth(BANDWIDTH_MAX); // Raise MTU so 24-byte telemetry fits in one notify
|
|
|
|
Bluefruit.begin(1, 0);
|
|
Bluefruit.setTxPower(4);
|
|
Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
|
|
Bluefruit.Periph.setConnInterval(CONN_INTERVAL_MIN, CONN_INTERVAL_MAX);
|
|
Bluefruit.Periph.setConnSlaveLatency(CONN_SLAVE_LATENCY);
|
|
Bluefruit.Periph.setConnectCallback(onConnect);
|
|
|
|
Wire1.begin(); // LSM6DS3 is on internal I2C bus (Wire1), must init before imu.begin()
|
|
// Keep the library's scaling in sync with imuConfigure() (sleep.cpp uses readFloatGyro*)
|
|
imu.settings.gyroRange = 1000;
|
|
imu.settings.gyroSampleRate = 208;
|
|
imu.settings.accelRange = 4;
|
|
imu.settings.accelSampleRate = 208;
|
|
imu.settings.accelODROff = 0;
|
|
if (imu.begin() != 0) {
|
|
Serial.println("[ERROR] IMU init failed");
|
|
while(1) { digitalWrite(LED_RED, !digitalRead(LED_RED)); delay(100); } // fault: red rapid blink
|
|
}
|
|
imuConfigure();
|
|
Serial.println("[OK] IMU ready");
|
|
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
if (cfg.featureFlags & FLAG_TAP_ENABLED) setupTapDetection();
|
|
#endif
|
|
|
|
#ifdef FEATURE_PHYSICAL_BUTTONS
|
|
setupPhysicalButtons();
|
|
#endif
|
|
|
|
cachedTempC = readIMUTemp();
|
|
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
initBatteryADC();
|
|
updateBattery();
|
|
#endif
|
|
|
|
calibrateGyroBias();
|
|
|
|
sleepManagerInit();
|
|
|
|
bledis.setManufacturer("Seeed Studio");
|
|
bledis.setModel("XIAO nRF52840 Sense");
|
|
bledis.begin();
|
|
|
|
blehid.begin();
|
|
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
blebas.begin(); blebas.write(batteryPercent(readBatteryVoltage()));
|
|
#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
|
|
#ifdef FEATURE_PHYSICAL_BUTTONS
|
|
Serial.print(" PHYSBTN");
|
|
#endif
|
|
Serial.print(" SLEEP");
|
|
Serial.println();
|
|
|
|
bootStartMs = millis();
|
|
lastMotionMs = lastImuPoll = lastBattTime = lastHeartbeat = lastTelemetry = millis();
|
|
}
|
|
|
|
|
|
// Debug state (serial commands)
|
|
static bool statsEnabled = false; // 's' - 1 Hz pipeline stats
|
|
static bool rawLogEnabled = false; // 'l' - every FIFO sample as CSV
|
|
static unsigned long diagUntil = 0; // 'd' - 10 s of yaw/pitch/roll
|
|
static uint32_t statHidReports = 0, statHidFails = 0, statSamples = 0;
|
|
static int32_t statSentX = 0, statSentY = 0;
|
|
static uint32_t statMaxHidGapUs = 0;
|
|
// 't' - synthetic sweep, totals reported when it ends
|
|
static bool testRunning = false;
|
|
static int32_t testPosX = 0, testNegX = 0, testPosY = 0, testNegY = 0;
|
|
static uint32_t testReports = 0, testFails = 0;
|
|
|
|
static void printStats(unsigned long now) {
|
|
const MotionDebug& m = motionDebug();
|
|
BLEConnection* c = Bluefruit.connected() ? Bluefruit.Connection(Bluefruit.connHandle()) : nullptr;
|
|
Serial.print("[STAT] smp/s="); Serial.print(statSamples);
|
|
Serial.print(" ovr="); Serial.print(imuFifoOverruns);
|
|
Serial.print(" i2cErr="); Serial.print(imuI2cErrors);
|
|
Serial.print(" hid/s="); Serial.print(statHidReports);
|
|
Serial.print(" fail="); Serial.print(statHidFails);
|
|
Serial.print(" sent="); Serial.print(statSentX); Serial.print(","); Serial.print(statSentY);
|
|
Serial.print(" maxGap="); Serial.print(statMaxHidGapUs / 1000.0f, 1); Serial.print("ms");
|
|
Serial.print(" ci="); if (c) { Serial.print(c->getConnectionInterval() * 1.25f, 2); Serial.print("ms"); } else Serial.print("-");
|
|
Serial.print(" bias(mdps)=");
|
|
for (int i = 0; i < 3; i++) { Serial.print(m.bias[i] * (180.f / PI) * 1000.f, 1); if (i < 2) Serial.print(","); }
|
|
Serial.print(m.biasValid ? " valid" : " UNCAL");
|
|
Serial.print(m.still ? " still" : " moving");
|
|
Serial.print(" upd="); Serial.print(m.biasUpdates);
|
|
Serial.print(" roll="); Serial.print(m.rollDeg, 1);
|
|
Serial.print(" up="); Serial.print(m.up[0], 3); Serial.print(","); Serial.print(m.up[1], 3); Serial.print(","); Serial.print(m.up[2], 3);
|
|
Serial.print(" T="); Serial.println(cachedTempC, 2);
|
|
statSamples = statHidReports = statHidFails = 0;
|
|
statMaxHidGapUs = 0;
|
|
(void)now;
|
|
}
|
|
|
|
// 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
|
|
|
|
// Serial commands: 'c' calibrate, 'r' factory reset, 'd' axis diagnostic,
|
|
// 's' toggle stats, 'l' toggle raw sample log, 't' synthetic sweep test
|
|
while (Serial.available()) {
|
|
char cmd = Serial.read();
|
|
if (cmd == 'c') { Serial.println("[SERIAL] Calibrate"); pendingCal = true; }
|
|
if (cmd == 'r') { Serial.println("[SERIAL] Reset"); pendingReset = true; }
|
|
if (cmd == 'd') { Serial.println("[DIAG] yaw/pitch (rad/s, roll-compensated) + roll for 10s"); diagUntil = now + 10000; }
|
|
if (cmd == 's') { statsEnabled = !statsEnabled; Serial.println(statsEnabled ? "[STAT] on" : "[STAT] off"); }
|
|
if (cmd == 'l') { rawLogEnabled = !rawLogEnabled; Serial.println(rawLogEnabled ? "[RAW] gx,gy,gz,ax,ay,az (LSB),roll (0.1 deg)" : "[RAW] off"); }
|
|
if (cmd == 't' && !testRunning) {
|
|
Serial.println("[TEST] Synthetic sweep: right, left, down, up (1 s each)");
|
|
testPosX = testNegX = testPosY = testNegY = 0; testReports = testFails = 0;
|
|
testRunning = true; motionStartTest();
|
|
}
|
|
#ifdef FEATURE_OTA
|
|
if (cmd == 'o') { Serial.println("[SERIAL] OTA DFU"); pendingOTA = true; }
|
|
#endif
|
|
}
|
|
|
|
if (pendingCal) { pendingCal = false; calibrateGyroBias(); }
|
|
if (pendingReset) { pendingReset = false; factoryReset(); }
|
|
#ifdef FEATURE_OTA
|
|
if (pendingOTA) {
|
|
pendingOTA = false;
|
|
Serial.println("[OTA] Disconnecting BLE and entering bootloader DFU mode...");
|
|
Serial.flush();
|
|
// Gracefully close the BLE connection first so the host can detect the
|
|
// disconnect and be ready to see DfuTarg advertise after the reboot.
|
|
if (Bluefruit.connected()) {
|
|
Bluefruit.disconnect(0);
|
|
delay(300);
|
|
}
|
|
delay(200);
|
|
enterOTADfu(); // Adafruit nRF52 core: sets GPREGRET correctly and resets into bootloader OTA mode
|
|
}
|
|
#endif
|
|
|
|
// Ask the host for a short connection interval once the link has settled.
|
|
if (connParamPending && Bluefruit.connected() && now - connectedAtMs >= 2000) {
|
|
connParamPending = false;
|
|
uint16_t h = Bluefruit.connHandle();
|
|
BLEConnection* c = Bluefruit.Connection(h);
|
|
if (c) {
|
|
Serial.print("[BLE] Connection interval "); Serial.print(c->getConnectionInterval() * 1.25f, 2); Serial.println("ms");
|
|
if (c->getConnectionInterval() > CONN_INTERVAL_MAX) {
|
|
ble_gap_conn_params_t p = { CONN_INTERVAL_MIN, CONN_INTERVAL_MAX, CONN_SLAVE_LATENCY, BLE_GAP_CONN_SUPERVISION_TIMEOUT_MS / 10 };
|
|
uint32_t err = sd_ble_gap_conn_param_update(h, &p);
|
|
Serial.print("[BLE] Requested 7.5-11.25ms interval (err="); Serial.print(err); Serial.println(")");
|
|
}
|
|
}
|
|
}
|
|
|
|
// Heartbeat LED (non-blocking, so it can't delay HID reports)
|
|
static int heartbeatLed = -1;
|
|
if (now - lastHeartbeat >= HEARTBEAT_MS) {
|
|
lastHeartbeat = now;
|
|
heartbeatLed = Bluefruit.connected() ? LED_BLUE : LED_GREEN; // blue=BT connected, green=advertising
|
|
digitalWrite(heartbeatLed, LOW);
|
|
}
|
|
if (heartbeatLed >= 0 && now - lastHeartbeat >= (unsigned long)HEARTBEAT_DUR) {
|
|
digitalWrite(heartbeatLed, HIGH); heartbeatLed = -1;
|
|
}
|
|
|
|
#ifdef FEATURE_BATTERY_MONITOR
|
|
if (now - lastBattTime >= BATT_REPORT_MS) { lastBattTime = now; updateBattery(); }
|
|
#endif
|
|
|
|
#ifdef FEATURE_TAP_DETECTION
|
|
if (cfg.featureFlags & FLAG_TAP_ENABLED) processTaps(now);
|
|
#endif
|
|
|
|
#ifdef FEATURE_PHYSICAL_BUTTONS
|
|
processPhysicalButtons();
|
|
#endif
|
|
|
|
// Debug modes keep the device awake so it can be measured lying still.
|
|
bool debugAwake = statsEnabled || rawLogEnabled || testRunning;
|
|
if (debugAwake && sleepStage == SLEEP_IMU_LP) sleepManagerWakeIMU();
|
|
bool idle = !debugAwake && (now - lastMotionMs >= IDLE_MS);
|
|
{
|
|
bool idle_for_sleep = (sleepStage == SLEEP_IMU_LP) ? true : idle;
|
|
if (sleepManagerUpdate(now, idle_for_sleep, Bluefruit.connected())) return;
|
|
}
|
|
|
|
static ImuRaw lastSample = {};
|
|
if (now - lastImuPoll >= (unsigned long)IMU_POLL_MS) {
|
|
lastImuPoll = now;
|
|
|
|
static unsigned long lastTempMs = 0;
|
|
if (now - lastTempMs >= 1000) { lastTempMs = now; cachedTempC = readIMUTemp(); }
|
|
|
|
#ifdef FEATURE_TELEMETRY
|
|
if (!safeMode && (now - lastTelemetry >= TELEMETRY_MS)) {
|
|
lastTelemetry = now; pushTelemetry(now);
|
|
}
|
|
#endif
|
|
|
|
// Run the tracking pipeline on every sample the IMU produced since the last poll.
|
|
ImuRaw samples[32];
|
|
int n = imuReadFifo(samples, 32);
|
|
for (int i = 0; i < n; i++) {
|
|
motionProcess(samples[i], now);
|
|
if (rawLogEnabled) {
|
|
const ImuRaw& r = samples[i];
|
|
Serial.print(r.gx); Serial.print(','); Serial.print(r.gy); Serial.print(','); Serial.print(r.gz); Serial.print(',');
|
|
Serial.print(r.ax); Serial.print(','); Serial.print(r.ay); Serial.print(','); Serial.print(r.az); Serial.print(',');
|
|
Serial.println((int)lroundf(motionDebug().rollDeg * 10.0f));
|
|
}
|
|
}
|
|
if (n > 0) lastSample = samples[n - 1];
|
|
statSamples += n;
|
|
if (motionConsumeMoved()) lastMotionMs = now;
|
|
}
|
|
|
|
// HID report, paced just under the connection interval: every radio event gets
|
|
// a report (an occasional event carries two), none goes out empty. Counts that
|
|
// could not be sent stay in the accumulator, so no motion is ever dropped.
|
|
static int32_t streamMoveX = 0, streamMoveY = 0;
|
|
static uint32_t lastHidUs = 0;
|
|
{
|
|
bool conn = Bluefruit.connected();
|
|
uint32_t intervalUs = 10000;
|
|
if (conn) {
|
|
BLEConnection* c = Bluefruit.Connection(Bluefruit.connHandle());
|
|
if (c) intervalUs = c->getConnectionInterval() * 1250UL;
|
|
}
|
|
intervalUs = intervalUs * 7 / 8;
|
|
uint32_t nowUs = micros();
|
|
int8_t dx, dy;
|
|
if (nowUs - lastHidUs >= intervalUs && motionTakeCounts(dx, dy)) {
|
|
uint32_t gap = nowUs - lastHidUs;
|
|
lastHidUs = nowUs;
|
|
bool ok = !conn || blehid.mouseMove(dx, dy); // not connected: counts are discarded
|
|
if (ok) {
|
|
statHidReports++; statSentX += dx; statSentY += dy;
|
|
if (gap > statMaxHidGapUs && gap < 1000000UL) statMaxHidGapUs = gap;
|
|
streamMoveX += dx; streamMoveY += dy;
|
|
if (testRunning) {
|
|
testReports++;
|
|
if (dx > 0) testPosX += dx; else testNegX += dx;
|
|
if (dy > 0) testPosY += dy; else testNegY += dy;
|
|
}
|
|
} else {
|
|
motionReturnCounts(dx, dy); statHidFails++; testFails++;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (testRunning && !motionTestActive()) {
|
|
testRunning = false;
|
|
Serial.print("[TEST] done: +X="); Serial.print(testPosX); Serial.print(" -X="); Serial.print(testNegX);
|
|
Serial.print(" +Y="); Serial.print(testPosY); Serial.print(" -Y="); Serial.print(testNegY);
|
|
Serial.print(" reports="); Serial.print(testReports); Serial.print(" fails="); Serial.println(testFails);
|
|
}
|
|
|
|
if (statsEnabled) {
|
|
static unsigned long lastStat = 0;
|
|
if (now - lastStat >= 1000) { lastStat = now; printStats(now); }
|
|
}
|
|
|
|
// Axis diagnostic
|
|
if (diagUntil && now < diagUntil) {
|
|
static unsigned long lastDiagPrint = 0;
|
|
if (now - lastDiagPrint >= 100) { lastDiagPrint = now;
|
|
const MotionDebug& m = motionDebug();
|
|
Serial.print("[DIAG] yaw="); Serial.print(m.yaw, 3);
|
|
Serial.print(" pitch="); Serial.print(m.pitch, 3);
|
|
Serial.print(" roll="); Serial.print(m.rollDeg, 1);
|
|
Serial.print(" speed="); Serial.println(m.speed, 4);
|
|
}
|
|
} else if (diagUntil) { diagUntil = 0; Serial.println("[DIAG] Done"); }
|
|
|
|
#ifdef FEATURE_IMU_STREAM
|
|
if (!safeMode && imuStreamEnabled && Bluefruit.connected()
|
|
&& (now - lastImuStream >= IMU_STREAM_RATE_MS)) {
|
|
lastImuStream = now;
|
|
|
|
if (now < streamBackoffUntil) {
|
|
// Backing off - host TX buffer congested, skip to avoid 100ms block
|
|
} else {
|
|
const MotionDebug& m = motionDebug();
|
|
ImuPacket pkt;
|
|
int8_t mx = (int8_t)constrain(streamMoveX, -127, 127);
|
|
int8_t my = (int8_t)constrain(streamMoveY, -127, 127);
|
|
pkt.gyroX_mDPS = (int16_t)constrain(m.pitch * (180.f/PI) * 1000.f, -32000, 32000);
|
|
pkt.gyroZ_mDPS = (int16_t)constrain(m.yaw * (180.f/PI) * 1000.f, -32000, 32000);
|
|
pkt.accelX_mg = (int16_t)constrain(lastSample.ax * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
|
|
pkt.accelY_mg = (int16_t)constrain(lastSample.ay * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
|
|
pkt.accelZ_mg = (int16_t)constrain(lastSample.az * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
|
|
pkt.moveX = mx;
|
|
pkt.moveY = my;
|
|
pkt.flags = idle ? 0x01 : 0x00;
|
|
pkt._pad = 0;
|
|
if (cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt))) {
|
|
streamMoveX -= mx; streamMoveY -= my;
|
|
streamNotifyOk++;
|
|
streamConsecFails = 0;
|
|
} else {
|
|
streamNotifyFails++;
|
|
streamConsecFails++;
|
|
if (streamConsecFails >= STREAM_BACKOFF_THRESH) {
|
|
streamBackoffUntil = now + STREAM_BACKOFF_MS;
|
|
streamConsecFails = 0;
|
|
Serial.print("[STREAM] TX congested - backing off ");
|
|
Serial.print(STREAM_BACKOFF_MS); Serial.println("ms");
|
|
}
|
|
}
|
|
}
|
|
|
|
// Periodic stream health report every 10 seconds
|
|
if (now - lastStreamDiag >= 10000) {
|
|
lastStreamDiag = now;
|
|
Serial.print("[STREAM] ok="); Serial.print(streamNotifyOk);
|
|
Serial.print(" fail="); Serial.print(streamNotifyFails);
|
|
Serial.print(" rate="); Serial.print((streamNotifyOk * 1000UL) / 10000); Serial.println("pkt/s");
|
|
streamNotifyOk = 0; streamNotifyFails = 0;
|
|
}
|
|
}
|
|
if (!imuStreamEnabled) streamMoveX = streamMoveY = 0;
|
|
#endif
|
|
}
|