Arduino additions

arduino/IMU.md

@@ -0,0 +1,78 @@
+# WT61 IMU Quick Reference
+
+6-axis IMU (accelerometer + gyroscope) with onboard angle calculation via Kalman filter.
+
+## Serial Configuration
+
+| Setting | Factory Default | Our Config |
+|---------|-----------------|------------|
+| Baud rate | 115200 | 9600 |
+| Output rate | 100Hz | 20Hz |
+
+**Config command** (send at 115200 to switch to 9600/20Hz):
+```
+0xFF 0xAA 0x64
+```
+This is saved to flash - persists across power cycles.
+
+## Wiring (ATmega328P / AltSoftSerial)
+
+| WT61 Pin | Arduino Pin | Notes |
+|----------|-------------|-------|
+| TX | 8 (RX) | AltSoftSerial fixed pin |
+| RX | 9 (TX) | Only needed for config commands |
+| VCC | 5V | |
+| GND | GND | |
+
+## Packet Structure
+
+11 bytes per packet, continuous stream (3 packet types interleaved):
+
+```
+Byte 0:    0x55 (header)
+Byte 1:    Packet type
+Bytes 2-3: Value 0 (int16_t, little-endian)
+Bytes 4-5: Value 1
+Bytes 6-7: Value 2
+Bytes 8-9: Temperature (usually ignored)
+Byte 10:   Checksum (sum of bytes 0-9, lower 8 bits)
+```
+
+### Packet Types
+
+| Type | Byte 1 | V0 | V1 | V2 |
+|------|--------|----|----|-----|
+| Acceleration | 0x51 | ax | ay | az |
+| Gyroscope | 0x52 | gx | gy | gz |
+| Angle | 0x53 | roll | pitch | yaw |
+
+## Scale Factors
+
+| Measurement | Formula | Range |
+|-------------|---------|-------|
+| Acceleration | `raw / 32768.0 * 16.0` | +/-16g |
+| Gyroscope | `raw / 32768.0 * 2000.0` | +/-2000 deg/s |
+| Angle | `raw / 32768.0 * 180.0` | +/-180 deg |
+| Temperature | `raw / 340.0 + 36.25` | Celsius |
+
+## Known Quirks
+
+- **Boot time**: Module needs ~200-500ms after power-on before sending valid data
+- **Config at wrong baud**: Commands sent at wrong baud rate are ignored (garbled bytes) - this is actually useful for idempotent config-on-boot
+- **AltSoftSerial at 115200**: Technically out of spec for 16MHz AVR, but TX-only bursts of a few bytes work fine. Don't try sustained RX at that rate.
+
+## Commands
+
+All commands are 3 bytes: `0xFF 0xAA <data>`
+
+| Data Byte | Function | Notes |
+|-----------|----------|-------|
+| 0x52 | Zero Z-axis angle | Resets yaw to 0 |
+| 0x67 | Accelerometer calibration | Keep module level, zeros X/Y |
+| 0x60 | Toggle sleep mode | Toggles between standby and active |
+| 0x61 | Serial mode | Enable UART, disable I2C |
+| 0x62 | I2C mode | Enable I2C, disable UART |
+| 0x63 | 115200 baud / 100Hz | Factory default |
+| 0x64 | 9600 baud / 20Hz | Our config |
+| 0x65 | Horizontal mounting | Module placed flat |
+| 0x66 | Vertical mounting | Module placed upright |

arduino/main/imu.cpp

@@ -2,7 +2,7 @@
 #include <AltSoftSerial.h>
 
 // AltSoftSerial uses fixed pins on ATmega328P:
-// RX = Pin 8, TX = Pin 9 (TX not used for WT61)
+// RX = Pin 8, TX = Pin 9
 static AltSoftSerial imuSerial;
 
 // WT61 packet structure:
@@ -23,6 +23,10 @@ static int rxIndex = 0;
 // Latest data
 static ImuData currentData = {0};
 
+// Calibration offsets and state
+static ImuData offsets = {0};
+static bool calibrated = false;
+
 // Scale factors from WT61 datasheet
 // Accel: raw / 32768 * 16g
 // Gyro:  raw / 32768 * 2000 deg/s
@@ -75,7 +79,19 @@ static void processPacket() {
 }
 
 void imu_init() {
-  imuSerial.begin(9600);
+  // 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
+  imuSerial.begin(115200);
+
+  /* uncomment after I wire up Nano D9 -> WT61 RX*/
+  // imu_send_cmd(0x64);  // 9600 baud / 20Hz
+  // delay(100);          // Let WT61 process and restart
+
+  // Switch to working baud rate
+  // imuSerial.begin(9600);
+  /* uncomment after I wire up Nano D9 -> WT61 RX*/
+
   rxIndex = 0;
   currentData = {0};
 }
@@ -107,9 +123,83 @@ bool imu_update() {
 }
 
 const ImuData& imu_get_data() {
+  // Apply calibration offsets if calibrated
+  // Using a static to avoid creating new struct each call
+  static ImuData calibratedData;
+
+  if (calibrated) {
+    calibratedData.ax = currentData.ax - offsets.ax;
+    calibratedData.ay = currentData.ay - offsets.ay;
+    calibratedData.az = currentData.az - offsets.az;
+    calibratedData.gx = currentData.gx - offsets.gx;
+    calibratedData.gy = currentData.gy - offsets.gy;
+    calibratedData.gz = currentData.gz - offsets.gz;
+    calibratedData.roll = currentData.roll - offsets.roll;
+    calibratedData.pitch = currentData.pitch - offsets.pitch;
+    calibratedData.yaw = currentData.yaw - offsets.yaw;
+    calibratedData.lastUpdate = currentData.lastUpdate;
+    return calibratedData;
+  }
+
   return currentData;
 }
 
 bool imu_is_fresh(unsigned long timeout_ms) {
   return (millis() - currentData.lastUpdate) < timeout_ms;
 }
+
+void imu_calibrate() {
+  const int SAMPLES = 5;  // ~250ms at 20Hz IMU rate
+
+  // Accumulators for averaging
+  float sum_ax = 0, sum_ay = 0, sum_az = 0;
+  float sum_gx = 0, sum_gy = 0, sum_gz = 0;
+  float sum_roll = 0, sum_pitch = 0, sum_yaw = 0;
+
+  int count = 0;
+  unsigned long lastUpdate = currentData.lastUpdate;
+
+  // Block until we've collected enough samples
+  while (count < SAMPLES) {
+    imu_update();
+
+    if (currentData.lastUpdate != lastUpdate) {
+      // New angle packet arrived (lastUpdate only changes on angle packets)
+      sum_ax += currentData.ax;
+      sum_ay += currentData.ay;
+      sum_az += currentData.az;
+      sum_gx += currentData.gx;
+      sum_gy += currentData.gy;
+      sum_gz += currentData.gz;
+      sum_roll += currentData.roll;
+      sum_pitch += currentData.pitch;
+      sum_yaw += currentData.yaw;
+
+      lastUpdate = currentData.lastUpdate;
+      count++;
+    }
+  }
+
+  // Store averaged offsets
+  offsets.ax = sum_ax / SAMPLES;
+  offsets.ay = sum_ay / SAMPLES;
+  offsets.az = sum_az / SAMPLES;
+  offsets.gx = sum_gx / SAMPLES;
+  offsets.gy = sum_gy / SAMPLES;
+  offsets.gz = sum_gz / SAMPLES;
+  offsets.roll = sum_roll / SAMPLES;
+  offsets.pitch = sum_pitch / SAMPLES;
+  offsets.yaw = sum_yaw / SAMPLES;
+
+  calibrated = true;
+}
+
+bool imu_is_calibrated() {
+  return calibrated;
+}
+
+void imu_send_cmd(uint8_t cmd) {
+  const uint8_t packet[] = {0xFF, 0xAA, cmd};
+  imuSerial.write(packet, 3);
+  imuSerial.flush();
+}

arduino/main/imu.h

@@ -28,4 +28,23 @@ const ImuData& imu_get_data();
 // Check if IMU data is fresh (updated within timeout_ms)
 bool imu_is_fresh(unsigned long timeout_ms = 200);
 
+// Calibrate IMU - blocks for ~250ms while sampling
+// Sets current orientation as zero reference
+// Note: Zeroes all axes including accel (loses gravity reference)
+//       Revisit once mounting orientation is finalized
+void imu_calibrate();
+
+// Check if calibration has been performed
+bool imu_is_calibrated();
+
+// Send command to IMU (see IMU.md for command list)
+// Common commands: 0x52 = zero yaw, 0x67 = calibrate accel
+void imu_send_cmd(uint8_t cmd);
+
+// Convenience: zero the yaw angle
+inline void imu_zero_yaw() { imu_send_cmd(0x52); }
+
+// Convenience: calibrate accelerometer (keep module level!)
+inline void imu_calibrate_accel() { imu_send_cmd(0x67); }
+
 #endif

arduino/main/main.ino

@@ -15,6 +15,14 @@ void setup() {
   voltage_init();
   imu_init();      // AltSoftSerial on pins 8(RX)/9(TX)
   comms_init();    // Hardware Serial to Pi at 115200
+
+  // Let IMU warm up a bit before calibrating
+  // (WT61 needs a moment to stabilize after power-on)
+  delay(500);
+
+  // Zero calibration - current position becomes reference
+  // Blocks for ~250ms while sampling
+  imu_calibrate();
 }
 
 void loop() {
@@ -71,8 +79,5 @@ void sendTelemetry() {
     comms_send("Yaw", imu.yaw);
   } else {
     comms_send("IMU", "STALE");
-
-    // flip LED to indicate main cycle
-    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
   }
 }