From b02e645655ae2a7c41ca577290ee23f66019277f Mon Sep 17 00:00:00 2001 From: sorcerdu Date: Mon, 11 May 2026 18:41:56 +0800 Subject: [PATCH 1/2] fix(sensor): correct soft-iron calibration matrix in-place overwrite The soft-iron compensation computed the 3x3 matrix-vector product in-place: each row overwrote magx/magy/magz before subsequent rows could read the original values. This introduced a heading-dependent bias that contributed to pan axis drift. Fix: use temporary variables for the full multiplication before writing back. --- firmware/src/src/sense.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp index 4f7d861f..b29072d3 100644 --- a/firmware/src/src/sense.cpp +++ b/firmware/src/src/sense.cpp @@ -1087,9 +1087,14 @@ void sensor_Thread() magy = rmagy - magyoff; magz = rmagz - magzoff; - magx = (magx * magsioff[0]) + (magy * magsioff[1]) + (magz * magsioff[2]); - magy = (magx * magsioff[3]) + (magy * magsioff[4]) + (magz * magsioff[5]); - magz = (magx * magsioff[6]) + (magy * magsioff[7]) + (magz * magsioff[8]); + // Apply Soft Iron Calibration Matrix + // Uses temp vars to avoid in-place overwrite corruption + float tmpmagx = (magx * magsioff[0]) + (magy * magsioff[1]) + (magz * magsioff[2]); + float tmpmagy = (magx * magsioff[3]) + (magy * magsioff[4]) + (magz * magsioff[5]); + float tmpmagz = (magx * magsioff[6]) + (magy * magsioff[7]) + (magz * magsioff[8]); + magx = tmpmagx; + magy = tmpmagy; + magz = tmpmagz; // Apply Rotation float tmpmag[3] = {magx, magy, magz}; From 33c774e7d96e042597b9506ca190395530689743 Mon Sep 17 00:00:00 2001 From: sorcerdu Date: Mon, 11 May 2026 18:41:56 +0800 Subject: [PATCH 2/2] feat(sensor): improve gyro bias estimation and add auto-recalibration MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The original EMA-based bias estimator (alpha=0.05) effectively only averaged the last ~20 samples, leaving significant noise in the bias estimate. Combined with the soft-iron bug, this caused ~0.1°/s cumulative drift on the pan axis during normal use. Changes: - Replace EMA with arithmetic mean over 1000 samples (~6.7s at 150Hz), reducing bias noise by ~31x - Add automatic re-calibration: when the device is stationary for >5s, silently re-estimate gyro bias (at most once per 60s) to compensate for thermal drift - Add manual re-calibration on center-reset button press - Clamp Madgwick filter delta-t to prevent integration overshoot from thread scheduling delays Tested on Seeed XIAO BLE Sense: pan drift reduced from ~0.1°/s to <1° over one hour of stationary operation. --- firmware/src/src/MadgwickAHRS/MadgwickAHRS.h | 5 + firmware/src/src/include/defines.h | 6 +- firmware/src/src/sense.cpp | 151 +++++++++++++++++-- 3 files changed, 146 insertions(+), 16 deletions(-) diff --git a/firmware/src/src/MadgwickAHRS/MadgwickAHRS.h b/firmware/src/src/MadgwickAHRS/MadgwickAHRS.h index f26d129f..0867e964 100644 --- a/firmware/src/src/MadgwickAHRS/MadgwickAHRS.h +++ b/firmware/src/src/MadgwickAHRS/MadgwickAHRS.h @@ -104,6 +104,11 @@ class Madgwick uint32_t dt = now - lastUpdate; lastUpdate = now; deltat = ((float)(dt) / 1000000.0f); + // Clamp to prevent integration overshoot from thread scheduling delays + // or 32-bit micros() wraparound (~71min). Normal period is ~6.67ms + // (SENSOR_PERIOD=6666us, 150Hz); cap at 100ms and fall back to the + // nominal rate. + if (deltat > 0.1f) deltat = 1.0f / 150.0f; return deltat; } }; diff --git a/firmware/src/src/include/defines.h b/firmware/src/src/include/defines.h index 762333f2..3d2f5cf4 100644 --- a/firmware/src/src/include/defines.h +++ b/firmware/src/src/include/defines.h @@ -164,8 +164,10 @@ // Magnetometer, Initial Orientation, Samples to average #define MADGSTART_SAMPLES 15 -#define GYRO_STABLE_SAMPLES 400 -#define GYRO_SAMPLE_WEIGHT 0.05f +// Number of samples averaged when estimating gyro zero-bias. +// Larger = lower noise on the bias estimate (~1/sqrt(N)), but longer wait. +// 1000 samples @150Hz ~= 6.7s, reduces noise by sqrt(1000)~=31x. +#define GYRO_STABLE_SAMPLES 1000 #define GYRO_FLASH_IF_OFFSET 0.5f // Save to flash if gyro is off more than 0.5 degrees/sec from flash value // Time macros diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp index b29072d3..1fd9b3e0 100644 --- a/firmware/src/src/sense.cpp +++ b/firmware/src/src/sense.cpp @@ -69,6 +69,11 @@ void gyroCalibrate(); void detectDoubleTap(); +void requestGyroRecalibrate(); +void gyroAutoRecalibrateCheck(); + +// Gyro re-calibration state (used by requestGyroRecalibrate / gyroAutoRecalibrateCheck) +static volatile bool gyroRecalRequested = false; static float auxdata[10]; static float raccx = 0, raccy = 0, raccz = 0; @@ -415,6 +420,12 @@ void calculate_Thread() rolloffset = roll; panoffset = pan; tiltoffset = tilt; + // Short press also triggers a gyro re-calibration. The calibration + // routine itself requires the device to be stable (gyro/acc rate of + // change within GYRO_STABLE_DIFF / ACC_STABLE_DIFF) for + // GYRO_STABLE_SAMPLES samples, so if the user is moving at the moment + // of pressing, it simply won't complete until they stop moving. + requestGyroRecalibrate(); butdnw = true; } @@ -1117,6 +1128,8 @@ void sensor_Thread() if (gyrValid) { gyroCalibrate(); + // Auto trigger gyro recalibration when device is stationary for long enough + gyroAutoRecalibrateCheck(); // If double tap detection is enabled, check for it if(trkset.getRstOnDbltTap()) detectDoubleTap(); @@ -1240,6 +1253,13 @@ void gyroCalibrate() { static float last_gyro_mag = 0; static float last_acc_mag = 0; + // Arithmetic-mean accumulators. Using double to avoid precision loss when + // summing thousands of small samples; this gives ~sqrt(N) noise reduction + // on the bias estimate, vs. the old EMA (alpha=0.05) which effectively + // only averaged the last ~20 samples. + static double sum_gyrx = 0; + static double sum_gyry = 0; + static double sum_gyrz = 0; static float filt_gyrx = 0; static float filt_gyry = 0; static float filt_gyrz = 0; @@ -1247,6 +1267,24 @@ void gyroCalibrate() static uint32_t filter_samples = 0; static uint64_t lasttime = 0; + // Handle external recalibration request - reset all internal state so the + // filter runs from scratch and captures a fresh bias estimate. + if (gyroRecalRequested) { + gyroRecalRequested = false; + last_gyro_mag = 0; + last_acc_mag = 0; + sum_gyrx = 0; + sum_gyry = 0; + sum_gyrz = 0; + filt_gyrx = 0; + filt_gyry = 0; + filt_gyrz = 0; + sent_gyro_cal_msg = false; + filter_samples = 0; + lasttime = 0; + gyroCalibrated = false; + } + if(gyroCalibrated) return; @@ -1268,21 +1306,23 @@ void gyroCalibrate() // Is Gyro anc Accelerometer stable? if (fabsf(gyro_dif) < GYRO_STABLE_DIFF && fabsf(acc_dif) < ACC_STABLE_DIFF) { - // First run, preload filter - if (filter_samples == 0) { - filt_gyrx = rgyrx; - filt_gyry = rgyry; - filt_gyrz = rgyrz; - sent_gyro_cal_msg = false; - filter_samples++; - } else if (filter_samples < GYRO_STABLE_SAMPLES) { - filt_gyrx = ((1.0f - GYRO_SAMPLE_WEIGHT) * filt_gyrx) + (GYRO_SAMPLE_WEIGHT * rgyrx); - filt_gyry = ((1.0f - GYRO_SAMPLE_WEIGHT) * filt_gyry) + (GYRO_SAMPLE_WEIGHT * rgyry); - filt_gyrz = ((1.0f - GYRO_SAMPLE_WEIGHT) * filt_gyrz) + (GYRO_SAMPLE_WEIGHT * rgyrz); + if (filter_samples < GYRO_STABLE_SAMPLES) { + // Accumulate; arithmetic mean over the whole window. + sum_gyrx += (double)rgyrx; + sum_gyry += (double)rgyry; + sum_gyrz += (double)rgyrz; filter_samples++; + if (filter_samples == 1) { + sent_gyro_cal_msg = false; + } } else if (filter_samples == GYRO_STABLE_SAMPLES) { - LOG_INF("Gyro Calibrated, x=%.3f,y=%.3f,z=%.3f", (double)filt_gyrx, (double)filt_gyry, - (double)filt_gyrz); + const double n = (double)GYRO_STABLE_SAMPLES; + filt_gyrx = (float)(sum_gyrx / n); + filt_gyry = (float)(sum_gyry / n); + filt_gyrz = (float)(sum_gyrz / n); + LOG_INF("Gyro Calibrated (mean of %u samples), x=%.4f,y=%.4f,z=%.4f", + (unsigned)GYRO_STABLE_SAMPLES, + (double)filt_gyrx, (double)filt_gyry, (double)filt_gyrz); gyroCalibrated = true; clearLEDFlag(LED_GYROCAL); // Set the new Gyro Offset Values @@ -1298,7 +1338,7 @@ void gyroCalibrate() fabsf(gyrzoff - filt_gyrz) > GYRO_FLASH_IF_OFFSET) { if (!sent_gyro_cal_msg) { k_sem_give(&saveToFlash_sem); - LOG_INF("Gyro calibration differs from saved value. Updating flash, x=%.3f,y=%.3f,z=%.3f", + LOG_INF("Gyro calibration differs from saved value. Updating flash, x=%.4f,y=%.4f,z=%.4f", (double)filt_gyrx, (double)filt_gyry, (double)filt_gyrz); sent_gyro_cal_msg = true; } @@ -1306,6 +1346,10 @@ void gyroCalibrate() filter_samples++; } } else { + // Movement detected mid-window: discard and restart clean. + sum_gyrx = 0; + sum_gyry = 0; + sum_gyrz = 0; filter_samples = 0; } @@ -1313,6 +1357,85 @@ void gyroCalibrate() // printk("%.4f,%.2f,%.2f\n", (float)time / 1000000.0f, gyro_dif, acc_dif); } +// Request a gyro re-calibration. Safe to call from any thread. +// The actual capture is performed by gyroCalibrate() on its next invocation +// inside sensor_Thread(), and only completes while the device is stable. +void requestGyroRecalibrate() +{ + gyroRecalRequested = true; + setLEDFlag(LED_GYROCAL); + LOG_INF("Gyro re-calibration requested"); +} + +// Automatic gyro re-calibration: whenever the device has been absolutely +// stationary (|raw gyro| small AND |raw acc|~=1g) for a few seconds, silently +// re-run the bias estimator. This compensates for slow bias drift that the +// original one-shot boot calibration can't track (e.g. residual warm-up, +// temperature drift, sensor aging). +void gyroAutoRecalibrateCheck() +{ + // Derive sensor rate from SENSOR_PERIOD to avoid hardcoding 150Hz + static const uint32_t SENSOR_HZ = 1000000UL / SENSOR_PERIOD; + // Coarse stillness pre-filter thresholds. Kept board-independent + // because they only gate the precise stability check inside + // gyroCalibrate(), which uses board-specific GYRO_STABLE_DIFF / + // ACC_STABLE_DIFF. Values here are deliberately generous so no + // platform is excluded: + // - 5 dps is above the worst-case bias on any supported sensor + // but well below any real head movement. + // - 0.05g covers mounting tilt and sensor noise without admitting + // acceleration from motion. + static const float STILL_GYRO_THRESH = 5.0f; // dps (raw, includes bias) + static const float STILL_ACC_THRESH = 0.05f; // g (|raw_acc|-1g) + static const uint32_t STILL_SAMPLES_REQ = 5 * SENSOR_HZ; // ~5s + // Rate-limit auto-recal to at most once per 5 minutes. Thermal bias + // drift is a minutes-scale process, and each recal briefly toggles + // gyroCalibrated=false, so firing too often would cause visible LED + // flicker during intermittent stationary periods. + static const uint64_t MIN_INTERVAL_MS = 5ULL * 60ULL * 1000ULL; + + // State kept across invocations + static uint32_t stillSamples = 0; + static uint64_t lastAutoGyroRecalTime_ms = 0; + + // Skip if an earlier request is still pending or initial calibration hasn't + // finished yet (gyroCalibrate() will run naturally in that case). + if (gyroRecalRequested || !gyroCalibrated) { + stillSamples = 0; + return; + } + + // Use RAW sensor values to avoid dependency on current bias estimate. + // rgyrx/rgyry/rgyrz include the true bias, so a stationary device will + // read ~bias (a few dps). The generous threshold accommodates this. + float gyroMag = sqrtf(rgyrx * rgyrx + rgyry * rgyry + rgyrz * rgyrz); + float accMag = sqrtf(raccx * raccx + raccy * raccy + raccz * raccz); + + bool still = (gyroMag < STILL_GYRO_THRESH) && + (fabsf(accMag - 1.0f) < STILL_ACC_THRESH); + + if (!still) { + stillSamples = 0; + return; + } + + stillSamples++; + if (stillSamples < STILL_SAMPLES_REQ) return; + + uint64_t now = millis64(); + if (lastAutoGyroRecalTime_ms != 0 && (now - lastAutoGyroRecalTime_ms) < MIN_INTERVAL_MS) { + // Too soon since last auto-recal; keep waiting but reset counter so we + // require another full still window. + stillSamples = 0; + return; + } + + LOG_INF("Auto gyro re-calibrate: stationary for >%ums", (unsigned)(STILL_SAMPLES_REQ * 1000 / SENSOR_HZ)); + lastAutoGyroRecalTime_ms = now; + stillSamples = 0; + requestGyroRecalibrate(); +} + // FROM https://stackoverflow.com/questions/1628386/normalise-orientation-between-0-and-360 // Normalizes any number to an arbitrary range // by assuming the range wraps around when going below min or above max