Arduino additions
This commit is contained in:
Mikkeli Matlock
78
arduino/IMU.md
Normal file
78
arduino/IMU.md
Normal file
@@ -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 |
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user