Add motion tracking and calibration features
This commit is contained in:
Nik Rozman committed
1 parent
4aa369ca73
commit
aab61a2b7b
13 files changed
+735
-169
No files matched your search
+199
-111
@@ -15,6 +15,7 @@
|
||||
|
||||
#include "config.h"
|
||||
#include "imu.h"
|
||||
#include "motion.h"
|
||||
#include "ble_config.h"
|
||||
#include "battery.h"
|
||||
#include "buttons.h"
|
||||
@@ -44,7 +45,7 @@ File cfgFile(InternalFS);
|
||||
// Config definitions
|
||||
Config cfg;
|
||||
const Config CFG_DEFAULTS = {
|
||||
CONFIG_MAGIC, 600.0f, 0.060f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW,
|
||||
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
|
||||
};
|
||||
@@ -55,10 +56,16 @@ TelemetryPacket telem = {};
|
||||
#endif
|
||||
|
||||
// Tuning constants
|
||||
const int LOOP_RATE_MS = 10;
|
||||
const float SMOOTH_ALPHA = 0.65f; // single-pole low-pass for cursor smoothing
|
||||
const int IMU_POLL_MS = 2; // FIFO poll period; samples arrive at 208 Hz
|
||||
const int BIAS_SAMPLES = 200;
|
||||
const int IDLE_FRAMES = 150;
|
||||
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;
|
||||
@@ -76,13 +83,8 @@ const float BATT_CRITICAL = 3.10f;
|
||||
#ifdef FEATURE_TEMP_COMPENSATION
|
||||
const float TEMP_COMP_COEFF_DPS_C = 0.004f;
|
||||
#endif
|
||||
#ifdef FEATURE_AUTO_RECAL
|
||||
const unsigned long AUTO_RECAL_MS = 5UL * 60UL * 1000UL;
|
||||
#endif
|
||||
|
||||
// Global state definitions
|
||||
float accumX = 0, accumY = 0;
|
||||
float biasGX = 0, biasGY = 0, biasGZ = 0;
|
||||
float calTempC = 25.0f;
|
||||
float cachedTempC = 25.0f;
|
||||
|
||||
@@ -108,7 +110,6 @@ float cachedTempC = 25.0f;
|
||||
const unsigned long STREAM_BACKOFF_MS = 500; // cooldown window
|
||||
#endif
|
||||
|
||||
uint32_t loopStalls = 0; // loop iterations where dt > 20ms (behind schedule)
|
||||
bool pendingCal = false;
|
||||
bool pendingReset = false;
|
||||
#ifdef FEATURE_OTA
|
||||
@@ -118,9 +119,8 @@ bool pendingReset = false;
|
||||
|
||||
ChargeStatus lastChargeStatus = CHGSTAT_DISCHARGING;
|
||||
|
||||
int idleFrames = 0;
|
||||
unsigned long idleStartMs = 0;
|
||||
unsigned long lastTime = 0;
|
||||
unsigned long lastMotionMs = 0;
|
||||
unsigned long lastImuPoll = 0;
|
||||
unsigned long lastBattTime = 0;
|
||||
unsigned long lastHeartbeat = 0;
|
||||
unsigned long lastTelemetry = 0;
|
||||
@@ -137,6 +137,15 @@ unsigned long bootStartMs = 0;
|
||||
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);
|
||||
@@ -190,13 +199,22 @@ 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(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
|
||||
@@ -270,7 +288,44 @@ void setup() {
|
||||
Serial.println();
|
||||
|
||||
bootStartMs = millis();
|
||||
lastTime = lastBattTime = lastHeartbeat = lastTelemetry = 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
|
||||
@@ -285,13 +340,20 @@ void loop() {
|
||||
}
|
||||
#endif
|
||||
|
||||
// Serial commands: 'c' = calibrate, 'r' = factory reset, 'd' = axis diagnostic
|
||||
static unsigned long diagUntil = 0;
|
||||
// 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] Printing raw gyro for 10s — pan, nod, roll one at a time"); diagUntil = now + 10000; }
|
||||
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
|
||||
@@ -315,11 +377,30 @@ void loop() {
|
||||
}
|
||||
#endif
|
||||
|
||||
// Heartbeat LED
|
||||
// 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;
|
||||
int led = Bluefruit.connected() ? LED_BLUE : LED_GREEN; // blue=BT connected, green=advertising
|
||||
digitalWrite(led, LOW); delay(HEARTBEAT_DUR); digitalWrite(led, HIGH);
|
||||
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
|
||||
@@ -333,101 +414,104 @@ void loop() {
|
||||
#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
|
||||
: (idleFrames >= IDLE_FRAMES);
|
||||
bool idle_for_sleep = (sleepStage == SLEEP_IMU_LP) ? true : idle;
|
||||
if (sleepManagerUpdate(now, idle_for_sleep, Bluefruit.connected())) return;
|
||||
}
|
||||
|
||||
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;
|
||||
// Threshold 50ms: intentional heartbeat blink (30ms) won't false-trigger;
|
||||
// real SoftDevice stalls (100ms+) and unexpected delays still get flagged.
|
||||
if (dt > 0.050f) { loopStalls++; Serial.print("[STALL] dt="); Serial.print(dt*1000.f,1); Serial.print("ms stalls="); Serial.println(loopStalls); }
|
||||
static ImuRaw lastSample = {};
|
||||
if (now - lastImuPoll >= (unsigned long)IMU_POLL_MS) {
|
||||
lastImuPoll = now;
|
||||
|
||||
cachedTempC = readIMUTemp();
|
||||
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);
|
||||
#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));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
if (n > 0) lastSample = samples[n - 1];
|
||||
statSamples += n;
|
||||
if (motionConsumeMoved()) lastMotionMs = now;
|
||||
}
|
||||
|
||||
// Gyro reads with optional temperature compensation.
|
||||
float correction = 0.0f;
|
||||
#ifdef FEATURE_TEMP_COMPENSATION
|
||||
if (cfg.featureFlags & FLAG_TEMP_COMP_ENABLED)
|
||||
correction = TEMP_COMP_COEFF_DPS_C * (cachedTempC - calTempC);
|
||||
#endif
|
||||
float gx = (imu.readFloatGyroX() - biasGX) * (PI/180.0f); // roll (unused for cursor)
|
||||
float gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f); // pitch → cursor Y
|
||||
float gz = (imu.readFloatGyroZ() - biasGZ - correction) * (PI/180.0f); // yaw → cursor X
|
||||
// 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++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Axis diagnostic — send 'd' over serial to enable
|
||||
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;
|
||||
Serial.print("[DIAG] gx="); Serial.print(gx,3);
|
||||
Serial.print(" gy="); Serial.print(gy,3);
|
||||
Serial.print(" gz="); Serial.println(gz,3);
|
||||
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"); }
|
||||
|
||||
// Direct axis mapping (empirically verified via diagnostic)
|
||||
float yawRate = gz; // gyroZ = pan left/right → cursor X
|
||||
float pitchRate = gy; // gyroY = nod up/down → cursor Y
|
||||
|
||||
// Dead zone (equal for both axes)
|
||||
float fYaw = (fabsf(yawRate) > cfg.deadZone) ? yawRate : 0.0f;
|
||||
float fPitch = (fabsf(pitchRate) > cfg.deadZone) ? pitchRate : 0.0f;
|
||||
|
||||
#ifdef DEBUG
|
||||
{ static unsigned long lastDiag = 0;
|
||||
if (now - lastDiag >= 500) { lastDiag = now;
|
||||
Serial.print("[IMU] gyro="); Serial.print(gx,2); Serial.print(","); Serial.print(gy,2); Serial.print(","); Serial.print(gz,2);
|
||||
Serial.print(" yaw="); Serial.print(yawRate,3); Serial.print(" pitch="); Serial.println(pitchRate,3);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
bool moving = (fPitch != 0.0f || fYaw != 0.0f);
|
||||
if (moving) { idleFrames = 0; idleStartMs = 0; }
|
||||
else { idleFrames++; if (idleStartMs == 0) idleStartMs = now; }
|
||||
bool idle = (idleFrames >= IDLE_FRAMES);
|
||||
|
||||
#ifdef FEATURE_AUTO_RECAL
|
||||
if ((cfg.featureFlags & FLAG_AUTO_RECAL_ENABLED) && idle && idleStartMs != 0 && (now - idleStartMs >= AUTO_RECAL_MS)) {
|
||||
Serial.println("[AUTO-CAL] Long idle - recalibrating...");
|
||||
idleStartMs = 0; calibrateGyroBias(); return;
|
||||
}
|
||||
#endif
|
||||
|
||||
int8_t moveX = 0, moveY = 0;
|
||||
uint8_t flags = 0;
|
||||
|
||||
static float smoothX = 0.0f, smoothY = 0.0f;
|
||||
|
||||
if (idle) {
|
||||
smoothX = smoothY = 0.0f;
|
||||
accumX = accumY = 0.0f;
|
||||
flags |= 0x01;
|
||||
} else {
|
||||
float rawX = applyAcceleration(applyCurve(-fYaw * cfg.sensitivity * dt));
|
||||
float rawY = applyAcceleration(applyCurve(-fPitch * cfg.sensitivity * dt));
|
||||
if (cfg.axisFlip & 0x01) rawX = -rawX;
|
||||
if (cfg.axisFlip & 0x02) rawY = -rawY;
|
||||
// Single-pole low-pass smoothing
|
||||
smoothX = smoothX * (1.0f - SMOOTH_ALPHA) + rawX * SMOOTH_ALPHA;
|
||||
smoothY = smoothY * (1.0f - SMOOTH_ALPHA) + rawY * SMOOTH_ALPHA;
|
||||
accumX += smoothX; accumY += smoothY;
|
||||
moveX = (int8_t)constrain((int)accumX, -127, 127);
|
||||
moveY = (int8_t)constrain((int)accumY, -127, 127);
|
||||
accumX -= moveX; accumY -= moveY;
|
||||
if (Bluefruit.connected() && (moveX != 0 || moveY != 0)) blehid.mouseMove(moveX, moveY);
|
||||
}
|
||||
|
||||
#ifdef FEATURE_IMU_STREAM
|
||||
if (!safeMode && imuStreamEnabled && Bluefruit.connected()
|
||||
&& (now - lastImuStream >= IMU_STREAM_RATE_MS)) {
|
||||
@@ -436,18 +520,21 @@ void loop() {
|
||||
if (now < streamBackoffUntil) {
|
||||
// Backing off - host TX buffer congested, skip to avoid 100ms block
|
||||
} else {
|
||||
float ax = imu.readFloatAccelX(), ay = imu.readFloatAccelY(), az = imu.readFloatAccelZ();
|
||||
const MotionDebug& m = motionDebug();
|
||||
ImuPacket pkt;
|
||||
pkt.gyroX_mDPS = (int16_t)constrain(gx*(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;
|
||||
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 {
|
||||
@@ -471,5 +558,6 @@ void loop() {
|
||||
streamNotifyOk = 0; streamNotifyFails = 0;
|
||||
}
|
||||
}
|
||||
if (!imuStreamEnabled) streamMoveX = streamMoveY = 0;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user