arduino: WT61 init logics & matched mounting

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)
 

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};
 }

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() {

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);
 }
 
+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_raw();
+  int raw = voltage_read_smoothed();
   float vDivider = (raw / (float)ADC_MAX) * ADC_REF;
   return vDivider / DIVIDER_RATIO + OFFSET;
 }

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