From 83cc6bed19c252258e7a134008e55e7fbe19381a Mon Sep 17 00:00:00 2001 From: Mikkeli Matlock Date: Mon, 2 Feb 2026 19:30:15 +0900 Subject: [PATCH] arduino: WT61 init logics & matched mounting --- arduino/IMU.md | 13 +++++++----- arduino/main/imu.cpp | 26 ++++++++++++++++++------ arduino/main/rpm.cpp | 13 ++++++++++-- arduino/main/voltage.cpp | 44 ++++++++++++++++++++++++++++++++++------ arduino/main/voltage.h | 11 ++++++++-- 5 files changed, 86 insertions(+), 21 deletions(-) diff --git a/arduino/IMU.md b/arduino/IMU.md index 105e2a6..0e9be53 100644 --- a/arduino/IMU.md +++ b/arduino/IMU.md @@ -6,14 +6,17 @@ | Setting | Factory Default | Our Config | |---------|-----------------|------------| -| Baud rate | 115200 | 9600 | -| Output rate | 100Hz | 20Hz | +| Baud rate | 115200 | 115200 | +| Output rate | 100Hz | 100Hz | -**Config command** (send at 115200 to switch to 9600/20Hz): +**Config commands** (sent on init): ``` -0xFF 0xAA 0x64 +0xFF 0xAA 0x66 # Vertical mounting mode +0xFF 0xAA 0x63 # 115200 baud / 100Hz ``` -This is saved to flash - persists across power cycles. +Settings are saved to flash - persist across power cycles. + +**Fallback to 9600/20Hz:** If 115200 causes packet loss on AltSoftSerial, change `0x63` to `0x64` in `imu.cpp` and add `imuSerial.begin(9600)` after the delay. ## Wiring (ATmega328P / AltSoftSerial) diff --git a/arduino/main/imu.cpp b/arduino/main/imu.cpp index 60cebd1..485500b 100644 --- a/arduino/main/imu.cpp +++ b/arduino/main/imu.cpp @@ -79,15 +79,29 @@ static void processPacket() { } void imu_init() { - // Send config at factory baud rate (115200) - // Sets WT61 to 9600/20Hz - see IMU.md for command reference - // Idempotent: if already at 9600, command is garbled and ignored + // Configure WT61 at 115200 - stays there (no baud switch) + // See IMU.md for command reference imuSerial.begin(115200); - imu_send_cmd(0x64); // 9600 baud / 20Hz - delay(100); // Let WT61 process and restart - // Switch to working baud rate + imu_send_cmd(0x52); // Reset yaw (for the sake of it) + delay(50); + imu_send_cmd(0x65); // Flat mounting mode + delay(50); + imu_send_cmd(0x64); // 9600 bauds / 20Hz report + delay(150); // Let WT61 process config + + // Revert to 9600 bauds imuSerial.begin(9600); + + // In case WT61 already is at 9600 + imu_send_cmd(0x52); // Reset yaw (for the sake of it) + delay(50); + imu_send_cmd(0x65); // Flat mounting mode + delay(50); + imu_send_cmd(0x64); // 9600 bauds / 20Hz report + delay(150); // Let WT61 process config + + rxIndex = 0; currentData = {0}; } diff --git a/arduino/main/rpm.cpp b/arduino/main/rpm.cpp index 2a8e220..1f23281 100644 --- a/arduino/main/rpm.cpp +++ b/arduino/main/rpm.cpp @@ -3,15 +3,24 @@ // Mock RPM: ramps up/down between idle and redline static int _rpm = 800; +static unsigned long _lastUpdate = 0; +static const unsigned long RPM_UPDATE_INTERVAL_MS = 100; // 10Hz ramp rate void rpm_init() { _rpm = 800; + _lastUpdate = 0; } void rpm_update() { - // ~100ms per call at 10Hz = takes ~7s to sweep range + unsigned long now = millis(); + if (now - _lastUpdate < RPM_UPDATE_INTERVAL_MS) { + return; // Not time yet + } + _lastUpdate = now; + + // +10 RPM every 100ms = ~7s to sweep 800-8000 _rpm += 10; - if (_rpm >= 8000) { _rpm = 800;} + if (_rpm >= 8000) { _rpm = 800; } } int rpm_get() { diff --git a/arduino/main/voltage.cpp b/arduino/main/voltage.cpp index 4cf37d5..6ebb30a 100644 --- a/arduino/main/voltage.cpp +++ b/arduino/main/voltage.cpp @@ -12,17 +12,49 @@ static const float ADC_REF = 5.0; static const int ADC_MAX = 1023; static const float OFFSET = 0.2; // calib +// Sliding window smoother (max 32 samples to keep RAM usage sane) +static const int MAX_WINDOW = 32; +static int _samples[MAX_WINDOW]; +static int _windowSize = 20; // Active window size +static int _sampleIndex = 0; +static long _sampleSum = 0; + void voltage_init() { - // analogRead doesn't need explicit pinMode, but here for future config - // e.g., could switch to internal 1.1V reference for different range + voltage_set_smoothing(20); // Default 20 samples +} + +void voltage_set_smoothing(int windowSize) { + // Clamp to valid range + if (windowSize < 1) windowSize = 1; + if (windowSize > MAX_WINDOW) windowSize = MAX_WINDOW; + _windowSize = windowSize; + + // Pre-fill window with current reading + int initial = analogRead(PIN_VBAT); + for (int i = 0; i < _windowSize; i++) { + _samples[i] = initial; + } + _sampleSum = (long)initial * _windowSize; + _sampleIndex = 0; } int voltage_read_raw() { return analogRead(PIN_VBAT); } -float voltage_read() { - int raw = voltage_read_raw(); - float vDivider = (raw / (float)ADC_MAX) * ADC_REF; - return vDivider / DIVIDER_RATIO + OFFSET; +int voltage_read_smoothed() { + int raw = analogRead(PIN_VBAT); + + _sampleSum -= _samples[_sampleIndex]; // Remove oldest + _samples[_sampleIndex] = raw; // Store new + _sampleSum += raw; // Add new + _sampleIndex = (_sampleIndex + 1) % _windowSize; + + return _sampleSum / _windowSize; +} + +float voltage_read() { + int raw = voltage_read_smoothed(); + float vDivider = (raw / (float)ADC_MAX) * ADC_REF; + return vDivider / DIVIDER_RATIO + OFFSET; } diff --git a/arduino/main/voltage.h b/arduino/main/voltage.h index 65c6d8a..1fd9c02 100644 --- a/arduino/main/voltage.h +++ b/arduino/main/voltage.h @@ -6,10 +6,17 @@ // Initialize voltage monitoring (call in setup) void voltage_init(); -// Read battery voltage, returns volts (e.g., 12.5) +// Set smoothing window size (1-32 samples, default 20) +// Resets the buffer with current reading +void voltage_set_smoothing(int windowSize); + +// Read battery voltage (smoothed), returns volts (e.g., 12.5) float voltage_read(); -// Read raw ADC value (0-1023) +// Read smoothed ADC value (averaged over window) +int voltage_read_smoothed(); + +// Read raw ADC value (0-1023), no smoothing int voltage_read_raw(); #endif