diff --git a/BirdsEye.ino b/BirdsEye.ino index 030d23c..d48bcec 100644 --- a/BirdsEye.ino +++ b/BirdsEye.ino @@ -97,6 +97,7 @@ int detectedTrackIndex = -1; unsigned long idleStartTime = 0; bool idleTimerRunning = false; bool newUiRaceActive = false; +unsigned long raceSessionStartedAt = 0; // For auto-idle grace period after RPM wake // Runtime settings (loaded from SD in setup) float settingLapDetectionDistance = 7.0; @@ -215,8 +216,7 @@ uint16_t bleMTUConnHandle = 0; // Connection handle for deferred MTU read /////////////////////////////////////////// const int tachInputPin = D0; -unsigned long tachLastUpdate = 0; -volatile int tachLastReported = 0; // Volatile for ISR/main-loop sharing +volatile int tachLastReported = 0; // Volatile: written by TACH_LOOP, read by display/logging/sleep int topTachReported = 0; // Debounce timing: ignore pulses faster than this (filters ignition ringing) @@ -224,24 +224,20 @@ int topTachReported = 0; static const uint32_t tachMinPulseGapUs = 3000; volatile uint32_t tachLastPulseUs = 0; -// Pulse period capture for RPM calculation (written by ISR, read by loop) -volatile uint32_t tachLastPeriodUs = 0; +// Sleep wake detection: set true by ISR on any valid pulse volatile bool tachHavePeriod = false; -// Software gate to prevent interrupt storm from noisy inductive pickup. -// After accepting a pulse, we ignore subsequent ISR calls for tachMinPulseGapUs. -// This is PURELY a volatile flag - we do NOT use noInterrupts() in the ISR -// because that would disable ALL system interrupts and cause deadlocks. -volatile bool tachInterruptShouldProcess = true; +// Ring buffer: ISR writes pulse timestamps, TACH_LOOP reads and computes periods. +// Single-producer (ISR writes head), single-consumer (TACH_LOOP reads tail). +// 16 entries handles up to 20k RPM with 48ms of main-loop stall margin. +static const uint8_t TACH_RING_SIZE = 16; +volatile uint32_t tachRingBuf[TACH_RING_SIZE]; +volatile uint8_t tachRingHead = 0; // ISR write index (only ISR writes) +static uint8_t tachRingTail = 0; // Main-loop read index (only TACH_LOOP writes) -// Filtered RPM state (updated in main loop, not ISR) -float tachRpmFiltered = 0.0f; - -// Tunable settings -const int tachUpdateRateHz = 3; -static const float tachRevsPerPulse = 1.0f; // Magneto 4T single: wasted spark = 1 pulse/rev -static const float tachFilterAlpha = 0.20f; // EMA filter: 0=smooth, 1=instant -static const uint32_t tachStopTimeoutUs = 500000; // 500ms with no pulse = engine stopped +// Tunable constants +static const float tachRevsPerPulse = 1.0f; // Wasted spark = 1 pulse/rev +static const uint32_t tachStopTimeoutUs = 500000; // 500ms = engine stopped /////////////////////////////////////////// // ACCELEROMETER GLOBALS @@ -898,7 +894,11 @@ void trackDetectionLoop() { activeTrackConfig.courseCount = 0; } - // Create CourseManager + // Create CourseManager (delete existing Lap Anything one if RPM-wake created it) + if (courseManager != nullptr) { + delete courseManager; + courseManager = nullptr; + } courseManager = new CourseManager(activeTrackConfig, crossingThresholdMeters); courseManager->setSpeedThresholdMph(settingWaypointSpeed); courseManager->setWaypointProximityMeters(settingWaypointDetectionDistance); @@ -979,6 +979,12 @@ void createLapAnythingCourseManager() { void checkAutoIdle() { if (!newUiRaceActive) return; + // Grace period: don't auto-idle within first 3 minutes of a session. + // After RPM wake the car is often stationary (warming up, waiting for + // track session) and GPS needs time to reacquire. Without this, the + // 60s idle timer kills the session before the driver even moves. + if (millis() - raceSessionStartedAt < 180000UL) return; + if (gps_speed_mph >= 2.0) { idleTimerRunning = false; idleStartTime = 0; @@ -1013,6 +1019,7 @@ void autoRaceModeCheck() { newUiRaceActive = true; enableLogging = true; trackSelected = true; // Allow GPS_LOOP to feed timer + log + raceSessionStartedAt = millis(); // Create a minimal CourseManager if none exists yet (no track detected) createLapAnythingCourseManager(); @@ -1152,6 +1159,7 @@ void exitSleepMode(bool rpmWake = false) { newUiRaceActive = true; enableLogging = true; trackSelected = true; + raceSessionStartedAt = millis(); createLapAnythingCourseManager(); switchToDisplayPage(TACHOMETER); } else { diff --git a/CLAUDE.md b/CLAUDE.md index 72fe5ef..2f08b56 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -141,9 +141,19 @@ loop() ~250 Hz - ISR `TACH_COUNT_PULSE()` fires on falling edge of D0. - 3 ms minimum pulse gap (supports up to ~20 000 RPM). -- Uses volatile flag gating instead of `noInterrupts()` to avoid deadlock. -- `TACH_LOOP()` applies EMA filter (alpha 0.20) and updates at 3 Hz. -- 500 ms timeout sets RPM to 0 (engine stopped). +- **Ring buffer architecture**: ISR timestamps every valid pulse into a + 16-entry ring buffer (`tachRingBuf`). `TACH_LOOP()` drains the buffer + each main-loop iteration, computes mean inter-pulse period from ALL + accumulated pulses, and feeds the result through a 1D Kalman filter. +- **Kalman filter** replaces the old median-of-3 + EMA. Two floats of + state (RPM estimate `kalmanX` + uncertainty `kalmanP`). Process noise + Q = 800 (tuned for kart engine inertia). Measurement noise R scales + inversely with pulse count (more pulses = more confident). +- Time-based debounce only (3 ms). Old volatile flag gate removed — ISR + body is trivially fast (<1 µs) and cannot cause interrupt storms. +- `tachLastReported` updates every main-loop call (~250 Hz). Consumers + (display at 3 Hz, logging at 25 Hz) rate-limit themselves. +- 500 ms timeout sets RPM to 0 (engine stopped), resets Kalman state. ### 3. Accelerometer (`accelerometer.ino`) @@ -410,9 +420,11 @@ Stored in `trackLayouts[MAX_LAYOUTS]` (max 10 per track). | DOVEX header size | 1 024 bytes | `project.h` | | Auto-idle timeout | 60 s at <2 mph | `BirdsEye.ino` | | Track detect radius | 5 miles | `BirdsEye.ino` | -| Tach min pulse gap | 3 ms | `tachometer.ino` | -| Tach EMA alpha | 0.20 | `tachometer.ino` | -| Tach stop timeout | 500 ms | `tachometer.ino` | +| Tach min pulse gap | 3 ms | `BirdsEye.ino` | +| Tach ring buffer | 16 entries | `BirdsEye.ino` | +| Tach Kalman Q | 800 RPM² | `tachometer.ino` | +| Tach Kalman R_BASE | 2500 RPM² | `tachometer.ino` | +| Tach stop timeout | 500 ms | `BirdsEye.ino` | | Display refresh | 3 Hz | `display_ui.ino` | | Button debounce | 200 ms | `display_ui.ino` | | SD SPI clock | 2 MHz | `BirdsEye.ino` | @@ -452,7 +464,8 @@ This device operates in ignition-noise environments. Three layers of defense: 2. **ISR design**: Volatile flag gating (never `noInterrupts()` in ISR); 3 ms minimum pulse gap in tachometer. 3. **Software**: Multi-sample button reads (3x at 500 us), 200 ms refire - lockout, EMA filtering on RPM, 2 MHz SPI clock for SD stability. + lockout, Kalman-filtered RPM (absorbs ISR jitter), 2 MHz SPI clock for + SD stability. 4. **GPS serial buffer**: TIMER3 ISR drains Serial1 into a 4 KB RAM ring buffer every 10 ms, preventing GPS data loss during SD card GC pauses that can block writes for 100 ms–2 s. diff --git a/TACHOMETER/tach_test_2.jpg b/TACHOMETER/tach_test_2.jpg index 0fe0a1e..93ca5d1 100644 Binary files a/TACHOMETER/tach_test_2.jpg and b/TACHOMETER/tach_test_2.jpg differ diff --git a/display_ui.ino b/display_ui.ino index c5a4658..afa1f71 100644 --- a/display_ui.ino +++ b/display_ui.ino @@ -274,6 +274,7 @@ void handleMenuPageSelection() { newUiRaceActive = true; enableLogging = true; trackSelected = true; + raceSessionStartedAt = millis(); // Create CourseManager if not already created by track detection createLapAnythingCourseManager(); switchToDisplayPage(GPS_SPEED); diff --git a/gps_functions.ino b/gps_functions.ino index 6b0b55a..21b83d5 100644 --- a/gps_functions.ino +++ b/gps_functions.ino @@ -565,6 +565,7 @@ void GPS_WAKE() { // Reset buffer pointers (stale data from before sleep is useless) gpsRxHead = 0; gpsRxTail = 0; + startGpsSerialTimer(); // Resume serial drain ISR myGNSS.checkUblox(); } diff --git a/tachometer.ino b/tachometer.ino index 85be9b6..3fb8eed 100644 --- a/tachometer.ino +++ b/tachometer.ino @@ -1,129 +1,164 @@ -/////////////////////////////////////////// -// TACHOMETER MODULE -// ISR and main loop processing for tachometer input -/////////////////////////////////////////// - -// After engine-stopped timeout, the first pulse period is garbage (it's the -// time since the last pulse before stopping, not a real RPM measurement). -// This flag tells TACH_LOOP to discard that first period. -static volatile bool tachNeedFirstPulseDiscard = true; - -// Median-of-3 filter: rejects single-pulse noise spikes from ignition -// ringing that slip past the debounce window. Two out of three readings -// must agree for a value to pass through to the EMA. -static uint32_t tachPeriodBuf[3]; -static uint8_t tachPeriodIdx = 0; -static uint8_t tachPeriodCount = 0; - -/** - * Tachometer ISR - called on falling edge of tach signal - * - * CRITICAL: This ISR must be fast and must NOT call noInterrupts(). - * Calling noInterrupts() here would disable ALL system interrupts, - * and if the main loop is delayed (e.g., SD write), interrupts would - * stay disabled forever, freezing button input and causing system lockup. - * - * Instead, we use a volatile flag gate (tachInterruptShouldProcess) that - * the main loop re-enables after the debounce period. - */ -void TACH_COUNT_PULSE() { - // Exit immediately if we're in the debounce window - if (!tachInterruptShouldProcess) return; - - uint32_t now = micros(); - uint32_t dt = now - tachLastPulseUs; - - // Secondary debounce check (belt and suspenders with the flag) - if (dt < tachMinPulseGapUs) return; - - // Record this pulse - tachLastPulseUs = now; - tachLastPeriodUs = dt; - tachHavePeriod = true; - - // Gate off further interrupts until main loop re-enables - // DO NOT call noInterrupts() here - that causes system-wide deadlock! - tachInterruptShouldProcess = false; -} - -/** - * Tachometer main loop processing - * - * Re-enables the ISR gate after debounce period, reads pulse data, - * applies exponential moving average filter, and handles timeout. - */ -void TACH_LOOP() { - // Re-enable interrupt processing after debounce window expires - if (!tachInterruptShouldProcess) { - uint32_t elapsed = micros() - tachLastPulseUs; - if (elapsed >= tachMinPulseGapUs) { - tachInterruptShouldProcess = true; - // Note: We don't call interrupts() here anymore - they were never disabled! - } - } - - // CRITICAL SECTION: Read-modify-clear of ISR shared data - // This DOES need noInterrupts() because: - // 1. Read tachHavePeriod - // 2. Read tachLastPeriodUs - // 3. Clear tachHavePeriod - // Without protection, ISR could fire between 2 and 3, writing a new - // period value that we'd then lose when we clear the flag. - uint32_t periodUs = 0; - bool havePeriod = false; - - noInterrupts(); - havePeriod = tachHavePeriod; - if (havePeriod) { - periodUs = tachLastPeriodUs; - tachHavePeriod = false; - } - interrupts(); - - // Apply median-of-3 filter then EMA to smooth RPM - if (havePeriod && periodUs > 0) { - // Discard first period after engine-stopped state. That period is - // the gap since the last pulse before stopping — not a real RPM. - if (tachNeedFirstPulseDiscard) { - tachNeedFirstPulseDiscard = false; - // Still update tachLastPulseUs (already done in ISR) so the NEXT - // period is measured from this pulse. Just don't feed filters. - } else { - // Feed period into median-of-3 ring buffer - tachPeriodBuf[tachPeriodIdx] = periodUs; - tachPeriodIdx = (tachPeriodIdx + 1) % 3; - if (tachPeriodCount < 3) tachPeriodCount++; - - // Need at least 3 samples for median - if (tachPeriodCount >= 3) { - // Median of 3 values (branchless-ish sort) - uint32_t a = tachPeriodBuf[0], b = tachPeriodBuf[1], c = tachPeriodBuf[2]; - uint32_t median; - if (a <= b) { - median = (b <= c) ? b : ((a <= c) ? c : a); - } else { - median = (a <= c) ? a : ((b <= c) ? c : b); - } - - float rpmInst = (60.0e6f * tachRevsPerPulse) / (float)median; - tachRpmFiltered += tachFilterAlpha * (rpmInst - tachRpmFiltered); - } - } - } - - // Timeout: if no pulses for tachStopTimeoutUs, engine is stopped - // Note: 32-bit reads are atomic on ARM Cortex-M4, no noInterrupts() needed - uint32_t lastPulseUs = tachLastPulseUs; - - if ((uint32_t)(micros() - lastPulseUs) > tachStopTimeoutUs) { - tachRpmFiltered = 0.0f; - tachNeedFirstPulseDiscard = true; - tachPeriodCount = 0; // Reset median buffer for next startup - } - - // Update display/log value at configured rate - if (millis() - tachLastUpdate > (1000 / tachUpdateRateHz)) { - tachLastUpdate = millis(); - tachLastReported = (int)(tachRpmFiltered + 0.5f); - } -} +/////////////////////////////////////////// +// TACHOMETER MODULE +// Ring buffer ISR + Kalman-filtered RPM from mean inter-pulse period +// +// Architecture: The ISR timestamps every valid falling edge into a 16-entry +// ring buffer. TACH_LOOP() drains the buffer, computes mean inter-pulse +// period from all accumulated pulses, and feeds the result through a 1D +// Kalman filter. This gives microsecond-resolution RPM from ALL pulses +// between reads, not just the most recent pair. +// +// At 5000 RPM (12ms period), 1us timestamp resolution gives ~0.008% RPM +// error. Mean of ~3 periods per 25Hz window further reduces noise. The +// Kalman filter smooths mechanical variation while tracking real RPM changes +// bounded by crankshaft inertia. +/////////////////////////////////////////// + +// ---- Kalman filter state ---- +static float kalmanX = 0.0f; // RPM estimate +static float kalmanP = 10000.0f; // Estimate uncertainty (RPM^2) + +// Process noise Q: how much RPM^2 can change between Kalman updates. +// A kart engine with light flywheel can shift ~200 RPM per pulse at 5k RPM. +// Q=800 is conservative-smooth. Increase to 1500-2000 if tracking feels sluggish. +static const float KALMAN_Q = 800.0f; + +// Measurement noise R_BASE: uncertainty of a single-period RPM measurement. +// ~50 RPM std dev from combustion variation + ISR latency jitter = variance 2500. +// Scales inversely with number of periods: more pulses → lower noise. +static const float KALMAN_R_BASE = 2500.0f; + +// After engine-stopped timeout, the first period is garbage (it spans the +// entire stopped duration). This flag discards it. +static volatile bool tachNeedFirstPulseDiscard = true; + +// Previous timestamp carried across TACH_LOOP calls for period calculation. +// When timestamp T_n is read in one call and T_{n+1} in the next, we need +// T_n to compute the period. +static uint32_t tachPrevTimestamp = 0; +static bool tachHavePrevTimestamp = false; + +/** + * Tachometer ISR - called on falling edge of tach signal (D0) + * + * Timestamps every valid pulse into a ring buffer. The 3ms time-based + * debounce is the sole protection against ignition ringing — the old + * volatile flag gate is removed because this ISR body is trivially fast + * (<1us, ~10 ARM instructions) and cannot cause interrupt-storm CPU issues. + */ +void TACH_COUNT_PULSE() { + uint32_t now = micros(); + uint32_t dt = now - tachLastPulseUs; + + // Time-based debounce: reject ignition ringing within 3ms of last valid pulse + if (dt < tachMinPulseGapUs) return; + + // Record timestamp + tachLastPulseUs = now; + tachRingBuf[tachRingHead] = now; + // ARM Cortex-M4: single-byte write is atomic. Data is visible before head + // advances because stores are observed in program order on same processor. + tachRingHead = (tachRingHead + 1) % TACH_RING_SIZE; + + // Wake trigger for sleep mode (BirdsEye.ino reads this) + tachHavePeriod = true; +} + +/** + * Tachometer main loop processing + * + * Drains the ring buffer, computes mean inter-pulse period from all + * accumulated timestamps, and updates the Kalman filter. Called every + * main loop iteration (~250Hz). No rate limiter — consumers (display + * at 3Hz, logging at 25Hz) rate-limit themselves. + */ +void TACH_LOOP() { + // ---- Step 1: Read new timestamps from ring buffer ---- + uint8_t head = tachRingHead; // Atomic byte read + + uint8_t available; + if (head >= tachRingTail) { + available = head - tachRingTail; + } else { + available = TACH_RING_SIZE - tachRingTail + head; + } + + if (available > 0) { + // Copy timestamps to local array + uint32_t ts[TACH_RING_SIZE]; + for (uint8_t i = 0; i < available; i++) { + ts[i] = tachRingBuf[(tachRingTail + i) % TACH_RING_SIZE]; + } + tachRingTail = head; // Consume all entries + + // ---- Step 2: Compute periods from consecutive timestamps ---- + uint32_t periods[TACH_RING_SIZE]; + uint8_t periodCount = 0; + + for (uint8_t i = 0; i < available; i++) { + if (tachHavePrevTimestamp) { + uint32_t dt = ts[i] - tachPrevTimestamp; // unsigned handles micros() wrap + + // First-pulse discard: after engine stop, the period from the last + // pre-stop pulse to the first new pulse spans the entire stopped + // duration — not a real RPM measurement. Discard it. + if (tachNeedFirstPulseDiscard) { + tachNeedFirstPulseDiscard = false; + tachPrevTimestamp = ts[i]; + continue; + } + + // Sanity bounds: 3ms (20k RPM) to 2s (30 RPM) + if (dt >= tachMinPulseGapUs && dt <= 2000000) { + periods[periodCount++] = dt; + } + } else { + // First timestamp ever — no period to compute yet + tachHavePrevTimestamp = true; + if (tachNeedFirstPulseDiscard) { + tachNeedFirstPulseDiscard = false; + } + } + tachPrevTimestamp = ts[i]; + } + + // ---- Step 3: Kalman filter update with all new periods ---- + if (periodCount > 0) { + // Mean period + uint32_t periodSum = 0; + for (uint8_t i = 0; i < periodCount; i++) { + periodSum += periods[i]; + } + float meanPeriodUs = (float)periodSum / (float)periodCount; + float rpmMeasured = (60.0e6f * tachRevsPerPulse) / meanPeriodUs; + + // Predict step: constant-RPM model, uncertainty grows + kalmanP += KALMAN_Q; + + // Measurement noise scales inversely with number of periods + float R = KALMAN_R_BASE / (float)periodCount; + + // Update step + float K = kalmanP / (kalmanP + R); + kalmanX += K * (rpmMeasured - kalmanX); + kalmanP *= (1.0f - K); + + // Uncertainty floor to prevent numerical collapse + if (kalmanP < 1.0f) kalmanP = 1.0f; + } + } + + // ---- Step 4: Engine-stopped timeout ---- + // 32-bit reads are atomic on ARM Cortex-M4, no noInterrupts() needed + uint32_t lastPulseUs = tachLastPulseUs; + if ((uint32_t)(micros() - lastPulseUs) > tachStopTimeoutUs) { + kalmanX = 0.0f; + kalmanP = 10000.0f; // High uncertainty for next startup + tachNeedFirstPulseDiscard = true; + tachHavePrevTimestamp = false; + tachRingTail = tachRingHead; // Flush ring buffer + } + + // ---- Step 5: Update reported value ---- + tachLastReported = (int)(kalmanX + 0.5f); +}