From f610f0fed290d554f1532be5394247f05eca5cfa Mon Sep 17 00:00:00 2001 From: Mikkeli Matlock Date: Sun, 1 Feb 2026 11:47:37 +0900 Subject: [PATCH] Arduino additions --- arduino/IMU.md | 78 +++++++++++++++++++++++++++++++++++ arduino/main/imu.cpp | 94 ++++++++++++++++++++++++++++++++++++++++++- arduino/main/imu.h | 19 +++++++++ arduino/main/main.ino | 11 +++-- 4 files changed, 197 insertions(+), 5 deletions(-) create mode 100644 arduino/IMU.md diff --git a/arduino/IMU.md b/arduino/IMU.md new file mode 100644 index 0000000..105e2a6 --- /dev/null +++ b/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 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 | diff --git a/arduino/main/imu.cpp b/arduino/main/imu.cpp index c5585a8..c73f2bf 100644 --- a/arduino/main/imu.cpp +++ b/arduino/main/imu.cpp @@ -2,7 +2,7 @@ #include // 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(); +} diff --git a/arduino/main/imu.h b/arduino/main/imu.h index fcac228..68b6159 100644 --- a/arduino/main/imu.h +++ b/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 diff --git a/arduino/main/main.ino b/arduino/main/main.ino index 355aece..880dcc3 100644 --- a/arduino/main/main.ino +++ b/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)); } }