Compare commits
10 Commits
71e2214e32
...
5cb0be0aaa
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
5cb0be0aaa | ||
|
|
18fbc63281 | ||
|
|
83cc6bed19 | ||
|
|
f7f0af92dd | ||
|
|
c1a2994d00 | ||
|
|
f1ed809c71 | ||
|
|
4e68dcef5f | ||
|
|
f610f0fed2 | ||
|
|
559e62e292 | ||
|
|
7a6e69861b |
2
.gitignore
vendored
2
.gitignore
vendored
@@ -65,6 +65,8 @@ scripts/*.pyo
|
||||
*.pyc
|
||||
*.pyo
|
||||
__pycache__/
|
||||
.venv/
|
||||
uv.lock
|
||||
|
||||
|
||||
# extra resources
|
||||
|
||||
3
arduino/.gitignore
vendored
Normal file
3
arduino/.gitignore
vendored
Normal file
@@ -0,0 +1,3 @@
|
||||
# arduino test files
|
||||
|
||||
test/
|
||||
81
arduino/IMU.md
Normal file
81
arduino/IMU.md
Normal file
@@ -0,0 +1,81 @@
|
||||
# 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 | 115200 |
|
||||
| Output rate | 100Hz | 100Hz |
|
||||
|
||||
**Config commands** (sent on init):
|
||||
```
|
||||
0xFF 0xAA 0x66 # Vertical mounting mode
|
||||
0xFF 0xAA 0x63 # 115200 baud / 100Hz
|
||||
```
|
||||
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)
|
||||
|
||||
| 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 |
|
||||
56
arduino/PROTOCOL.md
Normal file
56
arduino/PROTOCOL.md
Normal file
@@ -0,0 +1,56 @@
|
||||
# Arduino-Pi Communication Protocol
|
||||
|
||||
Telemetry protocol for Arduino → Pi communication over UART at 115200 baud.
|
||||
|
||||
## Design Rationale
|
||||
|
||||
- **ASCII-based**: Human-debuggable, digits are self-bounding (no accidental header spoofing)
|
||||
- **TSV format**: Tab delimiter, predictable field count, trivial to parse, survives floating decimals
|
||||
- **Null-terminated**: `\0` (0x00) is unambiguous end-of-line, avoids CRLF headaches
|
||||
- **10Hz rate**: ~50 bytes/line × 10Hz = 500 B/s, well under 115200 baud capacity (~4% utilization)
|
||||
|
||||
## Telemetry Frame (Arduino → Pi)
|
||||
|
||||
```
|
||||
field0\tfield1\tfield2\t...\tfieldN\0
|
||||
```
|
||||
|
||||
### Fields
|
||||
|
||||
| Index | Name | Unit | Description |
|
||||
|-------|--------|---------|--------------------------------|
|
||||
| 0 | V_bat | V | Battery voltage |
|
||||
| 1 | Ax | g | Acceleration X |
|
||||
| 2 | Ay | g | Acceleration Y |
|
||||
| 3 | Az | g | Acceleration Z |
|
||||
| 4 | Gx | deg/s | Angular velocity X |
|
||||
| 5 | Gy | deg/s | Angular velocity Y |
|
||||
| 6 | Gz | deg/s | Angular velocity Z |
|
||||
| 7 | Roll | deg | Euler angle roll |
|
||||
| 8 | Pitch | deg | Euler angle pitch |
|
||||
| 9 | Yaw | deg | Euler angle yaw |
|
||||
| 10 | RPM | RPM | Engine RPM |
|
||||
| 11 | Gear | - | Gear position (0=N, 1-6) |
|
||||
|
||||
### Example
|
||||
|
||||
```
|
||||
12.45\t0.02\t-0.01\t1.00\t0.50\t-0.25\t0.10\t2.35\t-1.20\t45.80\t3500\t3\0
|
||||
```
|
||||
|
||||
## Stale Data Handling
|
||||
|
||||
When IMU data is stale, empty fields are sent to preserve field count:
|
||||
```
|
||||
12.45\t\t\t\t\t\t\t\t\t\0
|
||||
```
|
||||
Backend parses empty fields as null/NaN.
|
||||
|
||||
## Commands (Pi → Arduino)
|
||||
|
||||
TBD: Command structure for configuration, calibration triggers, etc.
|
||||
|
||||
## Versioning
|
||||
|
||||
Protocol changes should bump a version field or use a different frame header.
|
||||
Currently unversioned (v0 / development).
|
||||
@@ -11,26 +11,45 @@ Sensor interface running on Arduino Nano, communicating with Pi via UART.
|
||||
## Current Capabilities
|
||||
|
||||
- Battery voltage monitoring (voltage divider on A0)
|
||||
- Serial output at 9600 baud, 1Hz update rate
|
||||
- WT61 IMU/gyro via AltSoftSerial (9-axis: accel, gyro, euler angles)
|
||||
- Duplex UART to Pi at 115200 baud, 10Hz telemetry output
|
||||
- Simple text-based protocol for easy debugging
|
||||
|
||||
## Planned
|
||||
## Dependencies
|
||||
|
||||
- RPM sensing (pulse counting from ignition coil)
|
||||
- Engine temperature (thermocouple/NTC)
|
||||
- Gear position indicator
|
||||
- Turn signal / high beam status
|
||||
Install via Arduino Library Manager:
|
||||
- **AltSoftSerial** by Paul Stoffregen - for WT61 IMU serial
|
||||
|
||||
## Pin Assignments
|
||||
|
||||
| Pin | Function |
|
||||
|-----|----------|
|
||||
| A0 | Battery voltage (via divider) |
|
||||
| D0 (RX) | Pi UART RX ← Arduino TX |
|
||||
| D1 (TX) | Pi UART TX → Arduino RX |
|
||||
| D8 | WT61 IMU RX (AltSoftSerial) |
|
||||
| D9 | WT61 IMU TX (unused, AltSoftSerial fixed pin) |
|
||||
| D13 | Status LED (heartbeat) |
|
||||
|
||||
## Hardware
|
||||
|
||||
- **MCU**: Arduino Nano (ATmega328P)
|
||||
- **Connection**: UART to Pi GPIO (TX→RX, RX→TX, common GND)
|
||||
- **Voltage sensing**: Resistor divider scaled for 0-20V input range
|
||||
- **Pi Connection**: UART at 115200 baud (TX→RX, RX→TX, common GND)
|
||||
- **IMU**: WT61 module at 9600 baud, 20Hz output
|
||||
- **Voltage sensing**: Resistor divider (100k/47k) scaled for 0-20V input
|
||||
|
||||
## Protocol
|
||||
|
||||
Simple text-based for now:
|
||||
```
|
||||
V_bat: 12.45V
|
||||
```
|
||||
TSV (tab-separated), null-terminated frames at 10Hz. See [PROTOCOL.md](PROTOCOL.md) for full specification.
|
||||
|
||||
Future: structured binary or JSON for multiple sensors.
|
||||
## Planned
|
||||
|
||||
- RPM sensing (pulse counting from ignition coil)
|
||||
- Gear position indicator
|
||||
|
||||
### Not planned
|
||||
|
||||
- Engine temperature (thermocouple/NTC)
|
||||
*Borderline do not want to do, simple but not really useful*
|
||||
- Turn signal / high beam status
|
||||
*No need to do something the dash already does*
|
||||
|
||||
113
arduino/main/comms.cpp
Normal file
113
arduino/main/comms.cpp
Normal file
@@ -0,0 +1,113 @@
|
||||
#include "comms.h"
|
||||
|
||||
// Pi communication uses hardware Serial (pins 0/1)
|
||||
// Baud rate - 115200 is reasonable for duplex with Pi
|
||||
static const long BAUD_RATE = 115200;
|
||||
|
||||
// Command buffer
|
||||
static const int CMD_BUF_SIZE = 64;
|
||||
static char cmdBuf[CMD_BUF_SIZE];
|
||||
static int cmdIndex = 0;
|
||||
static bool cmdReady = false;
|
||||
|
||||
// Connection tracking
|
||||
static unsigned long lastRxTime = 0;
|
||||
|
||||
void comms_init() {
|
||||
Serial.begin(BAUD_RATE);
|
||||
cmdIndex = 0;
|
||||
cmdReady = false;
|
||||
}
|
||||
|
||||
bool comms_update() {
|
||||
while (Serial.available()) {
|
||||
char c = Serial.read();
|
||||
lastRxTime = millis();
|
||||
|
||||
if (c == '\n' || c == '\r') {
|
||||
if (cmdIndex > 0) {
|
||||
cmdBuf[cmdIndex] = '\0';
|
||||
cmdReady = true;
|
||||
cmdIndex = 0;
|
||||
return true;
|
||||
}
|
||||
} else if (cmdIndex < CMD_BUF_SIZE - 1) {
|
||||
cmdBuf[cmdIndex++] = c;
|
||||
}
|
||||
// else: overflow, silently drop extra chars
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void comms_send_telemetry(float voltage, const ImuData& imu, bool imu_valid, int rpm, int gear) {
|
||||
// Field 0: voltage
|
||||
Serial.print(voltage, 2);
|
||||
Serial.write('\t');
|
||||
|
||||
if (imu_valid) {
|
||||
// Fields 1-3: acceleration
|
||||
Serial.print(imu.ax, 2);
|
||||
Serial.write('\t');
|
||||
Serial.print(imu.ay, 2);
|
||||
Serial.write('\t');
|
||||
Serial.print(imu.az, 2);
|
||||
Serial.write('\t');
|
||||
|
||||
// Fields 4-6: angular velocity
|
||||
Serial.print(imu.gx, 2);
|
||||
Serial.write('\t');
|
||||
Serial.print(imu.gy, 2);
|
||||
Serial.write('\t');
|
||||
Serial.print(imu.gz, 2);
|
||||
Serial.write('\t');
|
||||
|
||||
// Fields 7-9: euler angles
|
||||
Serial.print(imu.roll, 2);
|
||||
Serial.write('\t');
|
||||
Serial.print(imu.pitch, 2);
|
||||
Serial.write('\t');
|
||||
Serial.print(imu.yaw, 2);
|
||||
} else {
|
||||
// Empty fields for stale IMU (9 tabs for 9 empty fields)
|
||||
Serial.print(F("\t\t\t\t\t\t\t\t"));
|
||||
}
|
||||
|
||||
// Fields 10-11: RPM and gear
|
||||
Serial.write('\t');
|
||||
Serial.print(rpm);
|
||||
Serial.write('\t');
|
||||
Serial.print(gear);
|
||||
|
||||
// Null terminator (no newline)
|
||||
Serial.write('\0');
|
||||
}
|
||||
|
||||
void comms_send(const char* key, float value, int decimals) {
|
||||
Serial.print(key);
|
||||
Serial.print(": ");
|
||||
Serial.println(value, decimals);
|
||||
}
|
||||
|
||||
void comms_send(const char* key, int value) {
|
||||
Serial.print(key);
|
||||
Serial.print(": ");
|
||||
Serial.println(value);
|
||||
}
|
||||
|
||||
void comms_send(const char* key, const char* value) {
|
||||
Serial.print(key);
|
||||
Serial.print(": ");
|
||||
Serial.println(value);
|
||||
}
|
||||
|
||||
const char* comms_get_command() {
|
||||
if (cmdReady) {
|
||||
cmdReady = false;
|
||||
return cmdBuf;
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
bool comms_is_connected(unsigned long timeout_ms) {
|
||||
return (millis() - lastRxTime) < timeout_ms;
|
||||
}
|
||||
31
arduino/main/comms.h
Normal file
31
arduino/main/comms.h
Normal file
@@ -0,0 +1,31 @@
|
||||
#ifndef COMMS_H
|
||||
#define COMMS_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "imu.h"
|
||||
|
||||
// Initialize Pi serial communication (call in setup)
|
||||
void comms_init();
|
||||
|
||||
// Process incoming commands from Pi - call in loop
|
||||
// Returns true if a complete command was received
|
||||
bool comms_update();
|
||||
|
||||
// Send complete telemetry frame (TSV format, null-terminated)
|
||||
// Format: V_bat\tAx\tAy\tAz\tGx\tGy\tGz\tRoll\tPitch\tYaw\tRPM\tGear\0
|
||||
// If imu_valid is false, IMU fields are empty (but tabs preserved)
|
||||
void comms_send_telemetry(float voltage, const ImuData& imu, bool imu_valid, int rpm, int gear);
|
||||
|
||||
// Send key:value line (for debug/ACK, newline-terminated)
|
||||
void comms_send(const char* key, float value, int decimals = 2);
|
||||
void comms_send(const char* key, int value);
|
||||
void comms_send(const char* key, const char* value);
|
||||
|
||||
// Get last received command (empty if none)
|
||||
// Command buffer is cleared after reading
|
||||
const char* comms_get_command();
|
||||
|
||||
// Check if connected (received any data recently)
|
||||
bool comms_is_connected(unsigned long timeout_ms = 5000);
|
||||
|
||||
#endif
|
||||
19
arduino/main/gear.cpp
Normal file
19
arduino/main/gear.cpp
Normal file
@@ -0,0 +1,19 @@
|
||||
#include "gear.h"
|
||||
|
||||
// Mock gear: derived from RPM bands
|
||||
// Real sensor would read position switch
|
||||
|
||||
void gear_init() {
|
||||
// Nothing to init for mock
|
||||
}
|
||||
|
||||
int gear_get(int rpm) {
|
||||
// Simulate gear based on RPM
|
||||
// N < 1000, 1st < 2500, 2nd < 4000, 3rd < 5500, 4th < 7000, 5th+
|
||||
if (rpm < 1000) return 0; // Neutral
|
||||
if (rpm < 2500) return 1;
|
||||
if (rpm < 4000) return 2;
|
||||
if (rpm < 5500) return 3;
|
||||
if (rpm < 7000) return 4;
|
||||
return 5;
|
||||
}
|
||||
7
arduino/main/gear.h
Normal file
7
arduino/main/gear.h
Normal file
@@ -0,0 +1,7 @@
|
||||
#ifndef GEAR_H
|
||||
#define GEAR_H
|
||||
|
||||
void gear_init();
|
||||
int gear_get(int rpm); // Returns gear 0-6 (0=neutral)
|
||||
|
||||
#endif
|
||||
215
arduino/main/imu.cpp
Normal file
215
arduino/main/imu.cpp
Normal file
@@ -0,0 +1,215 @@
|
||||
#include "imu.h"
|
||||
#include <AltSoftSerial.h>
|
||||
|
||||
// AltSoftSerial uses fixed pins on ATmega328P:
|
||||
// RX = Pin 8, TX = Pin 9
|
||||
static AltSoftSerial imuSerial;
|
||||
|
||||
// WT61 packet structure:
|
||||
// Byte 0: 0x55 (header)
|
||||
// Byte 1: Packet type (0x51=accel, 0x52=gyro, 0x53=angle)
|
||||
// Bytes 2-9: Data (4x int16_t, little-endian)
|
||||
// Byte 10: Checksum (sum of bytes 0-9, lower 8 bits)
|
||||
static const uint8_t PACKET_HEADER = 0x55;
|
||||
static const uint8_t PACKET_ACCEL = 0x51;
|
||||
static const uint8_t PACKET_GYRO = 0x52;
|
||||
static const uint8_t PACKET_ANGLE = 0x53;
|
||||
static const int PACKET_SIZE = 11;
|
||||
|
||||
// Receive buffer
|
||||
static uint8_t rxBuf[PACKET_SIZE];
|
||||
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
|
||||
// Angle: raw / 32768 * 180 deg
|
||||
static const float ACCEL_SCALE = 16.0 / 32768.0;
|
||||
static const float GYRO_SCALE = 2000.0 / 32768.0;
|
||||
static const float ANGLE_SCALE = 180.0 / 32768.0;
|
||||
|
||||
static int16_t parseI16(uint8_t lo, uint8_t hi) {
|
||||
return (int16_t)((hi << 8) | lo);
|
||||
}
|
||||
|
||||
static bool validateChecksum() {
|
||||
uint8_t sum = 0;
|
||||
for (int i = 0; i < PACKET_SIZE - 1; i++) {
|
||||
sum += rxBuf[i];
|
||||
}
|
||||
return sum == rxBuf[PACKET_SIZE - 1];
|
||||
}
|
||||
|
||||
static void processPacket() {
|
||||
if (!validateChecksum()) {
|
||||
return; // Bad packet, ignore
|
||||
}
|
||||
|
||||
uint8_t type = rxBuf[1];
|
||||
int16_t v0 = parseI16(rxBuf[2], rxBuf[3]);
|
||||
int16_t v1 = parseI16(rxBuf[4], rxBuf[5]);
|
||||
int16_t v2 = parseI16(rxBuf[6], rxBuf[7]);
|
||||
// v3 at bytes 8-9 is temperature, ignored for now
|
||||
|
||||
switch (type) {
|
||||
case PACKET_ACCEL:
|
||||
currentData.ax = v0 * ACCEL_SCALE;
|
||||
currentData.ay = v1 * ACCEL_SCALE;
|
||||
currentData.az = v2 * ACCEL_SCALE;
|
||||
break;
|
||||
case PACKET_GYRO:
|
||||
currentData.gx = v0 * GYRO_SCALE;
|
||||
currentData.gy = v1 * GYRO_SCALE;
|
||||
currentData.gz = v2 * GYRO_SCALE;
|
||||
break;
|
||||
case PACKET_ANGLE:
|
||||
currentData.roll = v0 * ANGLE_SCALE;
|
||||
currentData.pitch = v1 * ANGLE_SCALE;
|
||||
currentData.yaw = v2 * ANGLE_SCALE;
|
||||
currentData.lastUpdate = millis();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void imu_init() {
|
||||
// Configure WT61 at 115200 - stays there (no baud switch)
|
||||
// See IMU.md for command reference
|
||||
imuSerial.begin(115200);
|
||||
|
||||
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};
|
||||
}
|
||||
|
||||
bool imu_update() {
|
||||
bool gotPacket = false;
|
||||
|
||||
while (imuSerial.available()) {
|
||||
uint8_t c = imuSerial.read();
|
||||
|
||||
// State machine: look for header, then collect packet
|
||||
if (rxIndex == 0) {
|
||||
if (c == PACKET_HEADER) {
|
||||
rxBuf[rxIndex++] = c;
|
||||
}
|
||||
// else: discard, wait for sync
|
||||
} else {
|
||||
rxBuf[rxIndex++] = c;
|
||||
|
||||
if (rxIndex >= PACKET_SIZE) {
|
||||
processPacket();
|
||||
rxIndex = 0;
|
||||
gotPacket = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return gotPacket;
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
50
arduino/main/imu.h
Normal file
50
arduino/main/imu.h
Normal file
@@ -0,0 +1,50 @@
|
||||
#ifndef IMU_H
|
||||
#define IMU_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// WT61 IMU data structure
|
||||
struct ImuData {
|
||||
// Acceleration (g)
|
||||
float ax, ay, az;
|
||||
// Angular velocity (deg/s)
|
||||
float gx, gy, gz;
|
||||
// Euler angles (degrees)
|
||||
float roll, pitch, yaw;
|
||||
// Timestamp of last valid packet (millis)
|
||||
unsigned long lastUpdate;
|
||||
};
|
||||
|
||||
// Initialize IMU serial (call in setup)
|
||||
void imu_init();
|
||||
|
||||
// Process incoming bytes - call frequently in loop
|
||||
// Returns true if new complete packet was parsed
|
||||
bool imu_update();
|
||||
|
||||
// Get latest IMU data
|
||||
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
|
||||
@@ -2,24 +2,82 @@
|
||||
// Motorcycle telemetry hub
|
||||
|
||||
#include "voltage.h"
|
||||
#include "imu.h"
|
||||
#include "rpm.h"
|
||||
#include "gear.h"
|
||||
#include "comms.h"
|
||||
|
||||
// Timing
|
||||
static const unsigned long TELEMETRY_INTERVAL_MS = 100; // 10Hz telemetry
|
||||
static unsigned long lastTelemetryTime = 0;
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
|
||||
comms_init(); // Hardware Serial first so we can debug
|
||||
Serial.println(F("[INIT] comms ok"));
|
||||
|
||||
voltage_init();
|
||||
Serial.println(F("[INIT] voltage ok"));
|
||||
|
||||
imu_init(); // AltSoftSerial on pins 8(RX)/9(TX)
|
||||
Serial.println(F("[INIT] imu ok"));
|
||||
|
||||
rpm_init();
|
||||
gear_init();
|
||||
Serial.println(F("[INIT] rpm/gear ok"));
|
||||
|
||||
// Let IMU warm up a bit before calibrating
|
||||
// (WT61 needs a moment to stabilize after power-on)
|
||||
delay(500);
|
||||
|
||||
Serial.println(F("[INIT] calibrating..."));
|
||||
// Zero calibration - current position becomes reference
|
||||
// Blocks for ~250ms while sampling
|
||||
imu_calibrate();
|
||||
Serial.println(F("[INIT] calibration done, entering loop"));
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Report battery voltage
|
||||
Serial.print("V_bat: ");
|
||||
Serial.print(voltage_read(), 2);
|
||||
Serial.println("V");
|
||||
// Always poll IMU - it's streaming at 20Hz
|
||||
imu_update();
|
||||
|
||||
// Heartbeat blink
|
||||
digitalWrite(LED_BUILTIN, HIGH);
|
||||
delay(50);
|
||||
digitalWrite(LED_BUILTIN, LOW);
|
||||
// Update mock RPM (ramping)
|
||||
rpm_update();
|
||||
|
||||
delay(1000); // 1Hz update rate
|
||||
// Process any commands from Pi
|
||||
if (comms_update()) {
|
||||
const char* cmd = comms_get_command();
|
||||
// Future: handle commands like "PING", "SET_RATE", etc.
|
||||
// For now, echo back as acknowledgment
|
||||
if (cmd[0] != '\0') {
|
||||
comms_send("ACK", cmd);
|
||||
}
|
||||
}
|
||||
|
||||
// Send telemetry at fixed interval
|
||||
unsigned long now = millis();
|
||||
if (now - lastTelemetryTime >= TELEMETRY_INTERVAL_MS) {
|
||||
lastTelemetryTime = now;
|
||||
sendTelemetry();
|
||||
}
|
||||
|
||||
// Heartbeat - quick blink if IMU fresh, slow blink if stale
|
||||
static unsigned long lastBlink = 0;
|
||||
unsigned long blinkInterval = imu_is_fresh() ? 500 : 2000;
|
||||
if (now - lastBlink >= blinkInterval) {
|
||||
lastBlink = now;
|
||||
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
|
||||
}
|
||||
}
|
||||
|
||||
void sendTelemetry() {
|
||||
// Send all telemetry in a single TSV frame
|
||||
float voltage = voltage_read();
|
||||
const ImuData& imu = imu_get_data();
|
||||
bool imu_valid = imu_is_fresh();
|
||||
int rpm = rpm_get();
|
||||
int gear = gear_get(rpm);
|
||||
|
||||
comms_send_telemetry(voltage, imu, imu_valid, rpm, gear);
|
||||
}
|
||||
|
||||
28
arduino/main/rpm.cpp
Normal file
28
arduino/main/rpm.cpp
Normal file
@@ -0,0 +1,28 @@
|
||||
#include "rpm.h"
|
||||
#include <Arduino.h>
|
||||
|
||||
// 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() {
|
||||
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; }
|
||||
}
|
||||
|
||||
int rpm_get() {
|
||||
return _rpm;
|
||||
}
|
||||
8
arduino/main/rpm.h
Normal file
8
arduino/main/rpm.h
Normal file
@@ -0,0 +1,8 @@
|
||||
#ifndef RPM_H
|
||||
#define RPM_H
|
||||
|
||||
void rpm_init();
|
||||
void rpm_update(); // Call in loop
|
||||
int rpm_get(); // Returns current RPM (0 if invalid)
|
||||
|
||||
#endif
|
||||
@@ -10,18 +10,51 @@ static const int PIN_VBAT = A0;
|
||||
static const float DIVIDER_RATIO = 47.0 / (100.0 + 47.0); // ~0.3197
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
"""Arduino service - connects to Arduino Nano via serial, buffers telemetry."""
|
||||
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
import threading
|
||||
import time
|
||||
@@ -17,7 +18,10 @@ except ImportError:
|
||||
class ArduinoService:
|
||||
"""Threaded Arduino serial reader with buffering and auto-reconnect."""
|
||||
|
||||
# Regex patterns for legacy text protocol
|
||||
# TSV field names (order per PROTOCOL.md)
|
||||
TSV_FIELDS = ['voltage', 'ax', 'ay', 'az', 'gx', 'gy', 'gz', 'roll', 'pitch', 'yaw', 'rpm', 'gear']
|
||||
|
||||
# Regex patterns for legacy text protocol (backwards compatibility)
|
||||
PATTERNS = {
|
||||
"voltage": re.compile(r"V_bat:\s*(\d+\.?\d*)V?", re.IGNORECASE),
|
||||
"rpm": re.compile(r"RPM:\s*(\d+)", re.IGNORECASE),
|
||||
@@ -30,7 +34,7 @@ class ArduinoService:
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str = "/dev/ttyUSB0",
|
||||
port: str = "/dev/serial0",
|
||||
baudrate: int = 115200,
|
||||
buffer_size: int = 100,
|
||||
):
|
||||
@@ -53,6 +57,10 @@ class ArduinoService:
|
||||
self._serial: Any = None
|
||||
self._serial_lock = threading.Lock()
|
||||
|
||||
# Periodic status logging
|
||||
self._last_status_log = 0.0
|
||||
self._frame_count = 0
|
||||
|
||||
def set_on_data(self, callback):
|
||||
"""Set callback for new telemetry data. Called with data dict."""
|
||||
self._on_data_callback = callback
|
||||
@@ -135,10 +143,8 @@ class ArduinoService:
|
||||
def _connect_and_read(self):
|
||||
"""Connect to Arduino serial and read data."""
|
||||
if serial is None:
|
||||
# Stub mode - no pyserial installed
|
||||
print("[Arduino] pyserial not installed, running in stub mode")
|
||||
self._stub_mode()
|
||||
return
|
||||
print("[Arduino] pyserial not installed, cannot connect")
|
||||
return # Will retry via _reader_loop after 5s
|
||||
|
||||
try:
|
||||
ser = serial.Serial(
|
||||
@@ -147,24 +153,26 @@ class ArduinoService:
|
||||
timeout=1.0,
|
||||
)
|
||||
except serial.SerialException as e:
|
||||
print(f"[Arduino] Cannot open {self.port}: {e}, falling back to stub mode")
|
||||
self._stub_mode()
|
||||
return
|
||||
print(f"[Arduino] Cannot open {self.port}: {e}")
|
||||
return # Will retry via _reader_loop after 5s
|
||||
|
||||
try:
|
||||
# Store serial handle for send_command()
|
||||
with self._serial_lock:
|
||||
self._serial = ser
|
||||
self._connected = True
|
||||
self._last_status_log = time.time()
|
||||
self._frame_count = 0
|
||||
print(f"[Arduino] Connected to {self.port} @ {self.baudrate} baud")
|
||||
|
||||
while self._running:
|
||||
try:
|
||||
line = ser.readline().decode("utf-8", errors="ignore").strip()
|
||||
# Read null-terminated line (TSV protocol)
|
||||
line = self._read_null_terminated(ser)
|
||||
if not line:
|
||||
continue
|
||||
|
||||
# Check for ACK responses first
|
||||
# Check for ACK responses first (legacy newline-terminated)
|
||||
ack_match = self.ACK_PATTERN.match(line)
|
||||
if ack_match:
|
||||
cmd, status, extra = ack_match.groups()
|
||||
@@ -178,7 +186,7 @@ class ArduinoService:
|
||||
with self._lock:
|
||||
# Merge new values into latest (preserve old values for partial updates)
|
||||
for key, val in data.items():
|
||||
if val is not None:
|
||||
if val is not None and not (isinstance(val, float) and math.isnan(val)):
|
||||
self._latest[key] = val
|
||||
self._latest["time"] = data["time"]
|
||||
self._buffer.append(self._latest.copy())
|
||||
@@ -187,6 +195,20 @@ class ArduinoService:
|
||||
if self._on_data_callback:
|
||||
self._on_data_callback(self._latest.copy())
|
||||
|
||||
# Periodic status log (every 5s)
|
||||
self._frame_count += 1
|
||||
now = time.time()
|
||||
if now - self._last_status_log >= 5.0:
|
||||
elapsed = now - self._last_status_log
|
||||
fps = self._frame_count / elapsed
|
||||
v = self._latest.get('voltage', 0)
|
||||
rpm = self._latest.get('rpm', 0)
|
||||
gear = self._latest.get('gear', 0)
|
||||
roll = self._latest.get('roll', 0)
|
||||
print(f"[Arduino] {fps:.1f} fps | V={v:.1f} RPM={int(rpm)} G={int(gear)} roll={roll:.1f}°")
|
||||
self._last_status_log = now
|
||||
self._frame_count = 0
|
||||
|
||||
except serial.SerialException as e:
|
||||
print(f"[Arduino] Serial error: {e}")
|
||||
break
|
||||
@@ -197,13 +219,40 @@ class ArduinoService:
|
||||
self._serial = None
|
||||
ser.close()
|
||||
|
||||
def _parse_line(self, line: str) -> dict[str, Any] | None:
|
||||
"""Parse a line from Arduino - JSON first, fallback to regex.
|
||||
def _read_null_terminated(self, ser) -> str:
|
||||
"""Read bytes until null terminator or newline (fallback for legacy)."""
|
||||
buf = bytearray()
|
||||
while self._running:
|
||||
byte = ser.read(1)
|
||||
if not byte:
|
||||
# Timeout
|
||||
if buf:
|
||||
# Return partial buffer if we have data
|
||||
return buf.decode("utf-8", errors="ignore").strip()
|
||||
return ""
|
||||
if byte == b'\x00' or byte == b'\n' or byte == b'\r':
|
||||
# End of frame
|
||||
if buf:
|
||||
return buf.decode("utf-8", errors="ignore").strip()
|
||||
# Skip empty lines / consecutive terminators
|
||||
continue
|
||||
buf.append(byte[0])
|
||||
# Safety limit
|
||||
if len(buf) > 256:
|
||||
return buf.decode("utf-8", errors="ignore").strip()
|
||||
|
||||
def _parse_line(self, line: str) -> dict[str, Any] | None:
|
||||
"""Parse a line from Arduino - TSV first, then JSON, fallback to regex.
|
||||
|
||||
TSV format: 12.45\t0.02\t-0.01\t... (10 fields, per PROTOCOL.md)
|
||||
JSON format: {"v":12.45,"rpm":4500,"eng":85,"gear":3}
|
||||
Legacy text: V_bat: 12.45V
|
||||
"""
|
||||
# Try JSON first (production format)
|
||||
# Try TSV first (new protocol)
|
||||
if '\t' in line:
|
||||
return self._parse_tsv(line)
|
||||
|
||||
# Try JSON (may still be used for special messages)
|
||||
try:
|
||||
obj = json.loads(line)
|
||||
return {
|
||||
@@ -225,31 +274,35 @@ class ArduinoService:
|
||||
|
||||
return result if result else None
|
||||
|
||||
def _stub_mode(self):
|
||||
"""Fake data for testing without Arduino connected."""
|
||||
import random
|
||||
def _parse_tsv(self, line: str) -> dict[str, Any] | None:
|
||||
"""Parse TSV telemetry frame per PROTOCOL.md.
|
||||
|
||||
_rpm = 3000
|
||||
Fields: voltage, ax, ay, az, gx, gy, gz, roll, pitch, yaw
|
||||
Empty fields (stale IMU) become NaN.
|
||||
"""
|
||||
fields = line.split('\t')
|
||||
if len(fields) != len(self.TSV_FIELDS):
|
||||
# Wrong field count - might be debug output or malformed
|
||||
return None
|
||||
|
||||
while self._running:
|
||||
self._connected = True
|
||||
data = {
|
||||
"time": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"voltage": round(12.0 + random.uniform(-0.5, 0.8), 2),
|
||||
"rpm": _rpm if random.random() > 0.1 else None,
|
||||
"eng_temp": random.randint(60, 95),
|
||||
"gear": random.randint(1, 6) if random.random() > 0.2 else 0, # 0 = neutral
|
||||
}
|
||||
_rpm += 10
|
||||
if _rpm > 7500:
|
||||
_rpm = 500
|
||||
result = {}
|
||||
for i, name in enumerate(self.TSV_FIELDS):
|
||||
val_str = fields[i].strip()
|
||||
if val_str == '':
|
||||
# Empty field = stale/missing data
|
||||
result[name] = float('nan')
|
||||
else:
|
||||
try:
|
||||
result[name] = float(val_str)
|
||||
except ValueError:
|
||||
result[name] = float('nan')
|
||||
|
||||
with self._lock:
|
||||
self._latest = data
|
||||
self._buffer.append(data)
|
||||
# IMU axis correction for mounting orientation
|
||||
# Pitch/yaw inverted for motorcycle frame alignment (roll left as-is)
|
||||
if 'pitch' in result and not math.isnan(result['pitch']):
|
||||
result['pitch'] = -result['pitch']
|
||||
if 'yaw' in result and not math.isnan(result['yaw']):
|
||||
result['yaw'] = -result['yaw']
|
||||
|
||||
# Invoke callback with new data
|
||||
if self._on_data_callback:
|
||||
self._on_data_callback(data)
|
||||
return result
|
||||
|
||||
time.sleep(0.5) # 2Hz stub updates
|
||||
|
||||
@@ -20,8 +20,8 @@ socketio = SocketIO(app, async_mode="gevent", cors_allowed_origins="*")
|
||||
gps = GPSService()
|
||||
arduino = ArduinoService()
|
||||
|
||||
# Throttles for emission rate limiting (2Hz for arduino, 1Hz for GPS)
|
||||
arduino_throttle = Throttle(min_interval=0.5) # 2Hz max
|
||||
# Throttles for emission rate limiting (20Hz for arduino, 1Hz for GPS)
|
||||
arduino_throttle = Throttle(min_interval=0.05) # 20Hz max
|
||||
gps_throttle = Throttle(min_interval=1.0) # 1Hz max
|
||||
|
||||
# Track connected clients
|
||||
@@ -157,7 +157,7 @@ def throttle_flusher():
|
||||
"""Periodically flush pending throttled data."""
|
||||
import gevent
|
||||
while True:
|
||||
gevent.sleep(0.5)
|
||||
gevent.sleep(0.05) # 20Hz flush rate
|
||||
|
||||
if arduino_throttle.has_pending:
|
||||
arduino_throttle.flush(lambda d: socketio.emit("arduino", d))
|
||||
|
||||
@@ -1,4 +1,6 @@
|
||||
import 'dart:convert';
|
||||
import 'package:flutter/material.dart';
|
||||
import 'package:http/http.dart' as http;
|
||||
|
||||
import 'screens/splash_screen.dart';
|
||||
import 'screens/dashboard_screen.dart';
|
||||
@@ -36,14 +38,12 @@ class _AppRootState extends State<AppRoot> {
|
||||
setState(() => _initStatus = 'Loading config...');
|
||||
await ConfigService.instance.load();
|
||||
|
||||
// Simulate init checks - replace with real checks later
|
||||
// (UART, GPS, sensors, etc.)
|
||||
|
||||
setState(() => _initStatus = 'Checking systems...');
|
||||
await Future.delayed(const Duration(milliseconds: 800));
|
||||
await Future.delayed(const Duration(milliseconds: 500));
|
||||
|
||||
setState(() => _initStatus = 'UART: standby');
|
||||
await Future.delayed(const Duration(milliseconds: 400));
|
||||
// Check UART connection via backend health endpoint
|
||||
setState(() => _initStatus = 'UART: connecting...');
|
||||
await _waitForUart();
|
||||
|
||||
setState(() => _initStatus = 'GPS: standby');
|
||||
await Future.delayed(const Duration(milliseconds: 400));
|
||||
@@ -61,6 +61,42 @@ class _AppRootState extends State<AppRoot> {
|
||||
setState(() => _initialized = true);
|
||||
}
|
||||
|
||||
/// Poll backend health endpoint until Arduino is connected
|
||||
Future<void> _waitForUart() async {
|
||||
final backendUrl = ConfigService.instance.backendUrl;
|
||||
const maxAttempts = 30; // ~30 seconds max wait
|
||||
const retryDelay = Duration(seconds: 1);
|
||||
|
||||
for (int attempt = 0; attempt < maxAttempts; attempt++) {
|
||||
try {
|
||||
final response = await http
|
||||
.get(Uri.parse('$backendUrl/health'))
|
||||
.timeout(const Duration(seconds: 2));
|
||||
|
||||
if (response.statusCode == 200) {
|
||||
final data = jsonDecode(response.body) as Map<String, dynamic>;
|
||||
final arduinoOk = data['arduino_connected'] == true;
|
||||
|
||||
if (arduinoOk) {
|
||||
setState(() => _initStatus = 'UART: OK');
|
||||
await Future.delayed(const Duration(milliseconds: 300));
|
||||
return;
|
||||
}
|
||||
}
|
||||
} catch (e) {
|
||||
// Backend not reachable yet - keep trying
|
||||
}
|
||||
|
||||
// Not connected yet
|
||||
setState(() => _initStatus = 'UART: waiting...');
|
||||
await Future.delayed(retryDelay);
|
||||
}
|
||||
|
||||
// Timeout - proceed anyway (UI will show stale data indicators)
|
||||
setState(() => _initStatus = 'UART: timeout');
|
||||
await Future.delayed(const Duration(milliseconds: 500));
|
||||
}
|
||||
|
||||
@override
|
||||
Widget build(BuildContext context) {
|
||||
// Determine which screen to show (priority: overheat > splash > dashboard)
|
||||
|
||||
@@ -10,6 +10,7 @@ import '../widgets/stat_box.dart';
|
||||
import '../widgets/stat_box_main.dart';
|
||||
import '../widgets/system_bar.dart';
|
||||
import '../widgets/debug_console.dart';
|
||||
import '../widgets/whiskey_mark.dart';
|
||||
|
||||
// test service for triggers
|
||||
import '../services/test_flipflop_service.dart';
|
||||
@@ -41,6 +42,8 @@ class _DashboardScreenState extends State<DashboardScreen> {
|
||||
double? _voltage;
|
||||
int? _engineTemp;
|
||||
int? _gear;
|
||||
double? _roll;
|
||||
double? _pitch;
|
||||
|
||||
// From backend - GPS data
|
||||
double? _gpsSpeed;
|
||||
@@ -66,6 +69,8 @@ class _DashboardScreenState extends State<DashboardScreen> {
|
||||
_rpm = data.rpm;
|
||||
_engineTemp = data.engTemp;
|
||||
_gear = data.gear;
|
||||
_roll = data.roll;
|
||||
_pitch = data.pitch;
|
||||
});
|
||||
});
|
||||
|
||||
@@ -99,6 +104,8 @@ class _DashboardScreenState extends State<DashboardScreen> {
|
||||
_rpm = cachedArduino.rpm;
|
||||
_engineTemp = cachedArduino.engTemp;
|
||||
_gear = cachedArduino.gear;
|
||||
_roll = cachedArduino.roll;
|
||||
_pitch = cachedArduino.pitch;
|
||||
}
|
||||
|
||||
final cachedGps = WebSocketService.instance.latestGps;
|
||||
@@ -175,12 +182,17 @@ class _DashboardScreenState extends State<DashboardScreen> {
|
||||
child: Row(
|
||||
children: [
|
||||
// RPM from Arduino
|
||||
StatBoxMain(
|
||||
value: _formatInt(_rpm),
|
||||
label: 'RPM',
|
||||
// StatBoxMain(
|
||||
// value: _formatInt(_rpm),
|
||||
// label: 'RPM',
|
||||
// ),
|
||||
// Attitude indicator (whiskey mark)
|
||||
Expanded(
|
||||
child: WhiskeyMark(
|
||||
roll: _roll,
|
||||
pitch: _pitch,
|
||||
),
|
||||
),
|
||||
// Add second StatBoxMain here for 2-up layout:
|
||||
// StatBoxMain(value: '4500', unit: 'rpm', label: 'TACH'),
|
||||
],
|
||||
),
|
||||
),
|
||||
@@ -191,8 +203,8 @@ class _DashboardScreenState extends State<DashboardScreen> {
|
||||
child: Row(
|
||||
mainAxisAlignment: MainAxisAlignment.spaceEvenly,
|
||||
children: [
|
||||
StatBox(value: _formatInt(_rpm), label: 'RPM'),
|
||||
StatBox(value: _formatInt(_engineTemp), unit: '°C', label: 'ENG'),
|
||||
StatBox(value: _formatInt(_rpm), label: 'RPM', isWarning: () => (_rpm ?? 0) > 4000),
|
||||
StatBox(value: _formatInt(_engineTemp), unit: '°C', label: 'ENG', isWarning: () => (_engineTemp ?? 0) > 120),
|
||||
StatBox(value: _formatGear(_gear), label: 'GEAR'),
|
||||
],
|
||||
),
|
||||
|
||||
@@ -2,14 +2,16 @@ import 'dart:async';
|
||||
import 'dart:convert';
|
||||
import 'package:http/http.dart' as http;
|
||||
|
||||
/// Data from Arduino (voltage, rpm, engine temp, gear)
|
||||
/// Data from Arduino (voltage, rpm, engine temp, gear, IMU)
|
||||
class ArduinoData {
|
||||
final double? voltage;
|
||||
final int? rpm;
|
||||
final int? engTemp;
|
||||
final int? gear; // 0 = neutral, 1-6 = gear
|
||||
final double? roll; // Euler angle in degrees (negative = left, positive = right)
|
||||
final double? pitch; // Euler angle in degrees (negative = nose down)
|
||||
|
||||
ArduinoData({this.voltage, this.rpm, this.engTemp, this.gear});
|
||||
ArduinoData({this.voltage, this.rpm, this.engTemp, this.gear, this.roll, this.pitch});
|
||||
|
||||
factory ArduinoData.fromJson(Map<String, dynamic> json) {
|
||||
return ArduinoData(
|
||||
@@ -17,6 +19,8 @@ class ArduinoData {
|
||||
rpm: (json['rpm'] as num?)?.toInt(),
|
||||
engTemp: (json['eng_temp'] as num?)?.toInt(),
|
||||
gear: (json['gear'] as num?)?.toInt(),
|
||||
roll: (json['roll'] as num?)?.toDouble(), // IMU mounted with axes swapped
|
||||
pitch: (json['pitch'] as num?)?.toDouble(),
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -86,4 +86,11 @@ class ConfigService {
|
||||
if (value is String && value.isNotEmpty) return value;
|
||||
return _defaultNavigator;
|
||||
}
|
||||
|
||||
/// Backend URL for API calls
|
||||
String get backendUrl {
|
||||
final value = _config?['backend_url'];
|
||||
if (value is String && value.isNotEmpty) return value;
|
||||
return 'http://127.0.0.1:5000';
|
||||
}
|
||||
}
|
||||
|
||||
@@ -184,7 +184,10 @@ class WebSocketService {
|
||||
final arduino = ArduinoData.fromJson(data);
|
||||
_latestArduino = arduino;
|
||||
_arduinoController.add(arduino);
|
||||
_log('ard: ${arduino.rpm ?? "-"}rpm ${arduino.voltage ?? "-"}V g${arduino.gear ?? "-"}');
|
||||
final rollStr = arduino.roll != null ? 'r${arduino.roll!.round()}' : '';
|
||||
final pitchStr = arduino.pitch != null ? 'p${arduino.pitch!.round()}' : '';
|
||||
final imuStr = (rollStr.isNotEmpty || pitchStr.isNotEmpty) ? ' $rollStr$pitchStr' : '';
|
||||
_log('ard: ${arduino.rpm ?? "-"}rpm ${arduino.voltage ?? "-"}V g${arduino.gear ?? "-"}$imuStr');
|
||||
}
|
||||
});
|
||||
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
import 'dart:io';
|
||||
import 'dart:math';
|
||||
import 'package:flutter/material.dart';
|
||||
import '../services/config_service.dart';
|
||||
|
||||
@@ -18,14 +19,34 @@ class NavigatorWidget extends StatefulWidget {
|
||||
State<NavigatorWidget> createState() => NavigatorWidgetState();
|
||||
}
|
||||
|
||||
class NavigatorWidgetState extends State<NavigatorWidget> {
|
||||
class NavigatorWidgetState extends State<NavigatorWidget>
|
||||
with SingleTickerProviderStateMixin {
|
||||
String _emotion = 'default';
|
||||
late AnimationController _shakeController;
|
||||
|
||||
@override
|
||||
void initState() {
|
||||
super.initState();
|
||||
_shakeController = AnimationController(
|
||||
duration: const Duration(milliseconds: 400),
|
||||
vsync: this,
|
||||
);
|
||||
}
|
||||
|
||||
@override
|
||||
void dispose() {
|
||||
_shakeController.dispose();
|
||||
super.dispose();
|
||||
}
|
||||
|
||||
/// Change the displayed emotion.
|
||||
/// Image file must exist at: {assetsPath}/navigator/{navigator}/{emotion}.png
|
||||
void setEmotion(String emotion) {
|
||||
if (emotion != _emotion) {
|
||||
setState(() => _emotion = emotion);
|
||||
if (emotion == 'surprise') {
|
||||
_shakeController.forward(from: 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -40,7 +61,7 @@ class NavigatorWidgetState extends State<NavigatorWidget> {
|
||||
final config = ConfigService.instance;
|
||||
final basePath = '${config.assetsPath}/navigator/${config.navigator}';
|
||||
|
||||
return Image.file(
|
||||
final image = Image.file(
|
||||
File('$basePath/$_emotion.png'),
|
||||
fit: BoxFit.contain,
|
||||
errorBuilder: (context, error, stackTrace) {
|
||||
@@ -55,5 +76,19 @@ class NavigatorWidgetState extends State<NavigatorWidget> {
|
||||
return const SizedBox.shrink();
|
||||
},
|
||||
);
|
||||
|
||||
// Shake animation for surprise
|
||||
return AnimatedBuilder(
|
||||
animation: _shakeController,
|
||||
child: image,
|
||||
builder: (context, child) {
|
||||
final shake = sin(_shakeController.value * pi * 6) * 10 *
|
||||
(1 - _shakeController.value); // 6 oscillations, 4px amplitude, decay
|
||||
return Transform.translate(
|
||||
offset: Offset(shake, 0),
|
||||
child: child,
|
||||
);
|
||||
},
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -9,17 +9,23 @@ class StatBox extends StatelessWidget {
|
||||
final String label;
|
||||
final int flex;
|
||||
|
||||
/// Optional warning predicate - if returns true, value shows in highlight color
|
||||
final bool Function()? isWarning;
|
||||
|
||||
const StatBox({
|
||||
super.key,
|
||||
required this.value,
|
||||
this.unit,
|
||||
required this.label,
|
||||
this.flex = 1,
|
||||
this.isWarning,
|
||||
});
|
||||
|
||||
@override
|
||||
Widget build(BuildContext context) {
|
||||
final theme = AppTheme.of(context);
|
||||
final warning = isWarning?.call() ?? false;
|
||||
final valueColor = warning ? theme.highlight : theme.foreground;
|
||||
|
||||
return Expanded(
|
||||
flex: flex,
|
||||
@@ -42,7 +48,7 @@ class StatBox extends StatelessWidget {
|
||||
fontSize: baseSize,
|
||||
fontWeight: FontWeight.w400,
|
||||
fontFeatures: const [FontFeature.tabularFigures()],
|
||||
color: theme.foreground,
|
||||
color: valueColor,
|
||||
height: 1,
|
||||
),
|
||||
),
|
||||
|
||||
233
pi/ui/lib/widgets/whiskey_mark.dart
Normal file
233
pi/ui/lib/widgets/whiskey_mark.dart
Normal file
@@ -0,0 +1,233 @@
|
||||
import 'dart:math' as math;
|
||||
import 'package:flutter/material.dart';
|
||||
|
||||
import '../theme/app_theme.dart';
|
||||
|
||||
/// Primitive attitude indicator (whiskey mark) displaying roll/pitch.
|
||||
///
|
||||
/// Visual: tilting horizon line based on roll angle
|
||||
/// Hard left (-45°+): ╲
|
||||
/// Left (-15°): ╲─
|
||||
/// Level (0°): ─
|
||||
/// Right (+15°): ─╱
|
||||
/// Hard right (+45°+): ╱
|
||||
///
|
||||
/// Below the horizon: numeric readout "R: -12° P: 5°"
|
||||
class WhiskeyMark extends StatelessWidget {
|
||||
/// Roll angle in degrees. Negative = left bank, positive = right bank.
|
||||
final double? roll;
|
||||
|
||||
/// Pitch angle in degrees. Negative = nose down, positive = nose up.
|
||||
final double? pitch;
|
||||
|
||||
const WhiskeyMark({
|
||||
super.key,
|
||||
this.roll,
|
||||
this.pitch,
|
||||
});
|
||||
|
||||
@override
|
||||
Widget build(BuildContext context) {
|
||||
final theme = AppTheme.of(context);
|
||||
|
||||
return LayoutBuilder(
|
||||
builder: (context, constraints) {
|
||||
final size = math.min(constraints.maxWidth, constraints.maxHeight);
|
||||
final horizonSize = size * 0.6;
|
||||
final fontSize = size * 0.12;
|
||||
|
||||
return Column(
|
||||
mainAxisAlignment: MainAxisAlignment.center,
|
||||
children: [
|
||||
// Horizon indicator
|
||||
SizedBox(
|
||||
width: horizonSize,
|
||||
height: horizonSize,
|
||||
child: CustomPaint(
|
||||
painter: _HorizonPainter(
|
||||
roll: roll ?? 0,
|
||||
pitch: pitch ?? 0,
|
||||
lineColor: theme.foreground,
|
||||
borderWeight: 8,
|
||||
skyColor: theme.subdued,
|
||||
groundColor: theme.background,
|
||||
),
|
||||
),
|
||||
),
|
||||
|
||||
SizedBox(height: size * 0.05),
|
||||
|
||||
// Numeric readout
|
||||
Row(
|
||||
mainAxisAlignment: MainAxisAlignment.center,
|
||||
children: [
|
||||
Text(
|
||||
'Roll: ${_formatAngle(roll)}',
|
||||
style: TextStyle(
|
||||
fontSize: fontSize * 0.8,
|
||||
fontWeight: FontWeight.w400,
|
||||
fontFeatures: const [FontFeature.tabularFigures()],
|
||||
color: theme.foreground,
|
||||
),
|
||||
),
|
||||
SizedBox(width: size * 0.1),
|
||||
Text(
|
||||
'P: ${_formatAngle(pitch)}',
|
||||
style: TextStyle(
|
||||
fontSize: fontSize * 0.8,
|
||||
fontWeight: FontWeight.w400,
|
||||
fontFeatures: const [FontFeature.tabularFigures()],
|
||||
color: theme.subdued,
|
||||
),
|
||||
),
|
||||
],
|
||||
),
|
||||
|
||||
// Label
|
||||
Text(
|
||||
'ATTITUDE',
|
||||
style: TextStyle(
|
||||
fontSize: fontSize * 0.8,
|
||||
fontWeight: FontWeight.w400,
|
||||
color: theme.subdued,
|
||||
letterSpacing: 1,
|
||||
),
|
||||
),
|
||||
],
|
||||
);
|
||||
},
|
||||
);
|
||||
}
|
||||
|
||||
String _formatAngle(double? angle) {
|
||||
if (angle == null) return '—°';
|
||||
return '${angle.round()}°';
|
||||
}
|
||||
}
|
||||
|
||||
/// Custom painter for the tilting horizon line
|
||||
class _HorizonPainter extends CustomPainter {
|
||||
final double roll;
|
||||
final double pitch;
|
||||
final double borderWeight;
|
||||
final Color lineColor;
|
||||
final Color skyColor;
|
||||
final Color groundColor;
|
||||
|
||||
_HorizonPainter({
|
||||
required this.roll,
|
||||
required this.pitch,
|
||||
required this.borderWeight,
|
||||
required this.lineColor,
|
||||
required this.skyColor,
|
||||
required this.groundColor,
|
||||
});
|
||||
|
||||
@override
|
||||
void paint(Canvas canvas, Size size) {
|
||||
final center = Offset(size.width / 2, size.height / 2);
|
||||
final radius = math.min(size.width, size.height) / 2;
|
||||
|
||||
// Clip to circle
|
||||
canvas.save();
|
||||
canvas.clipPath(Path()..addOval(Rect.fromCircle(center: center, radius: radius)));
|
||||
|
||||
// Convert roll to radians (negate so positive roll tilts right visually)
|
||||
final rollRad = -roll * math.pi / 180;
|
||||
|
||||
// Pitch offset (positive pitch moves horizon down, showing more sky)
|
||||
// Scale: 90° pitch = full radius displacement
|
||||
final pitchOffset = (pitch / 90) * radius;
|
||||
|
||||
// Calculate horizon line endpoints
|
||||
// The horizon is a horizontal line that we rotate by roll and offset by pitch
|
||||
final horizonY = center.dy + pitchOffset;
|
||||
|
||||
// Paint sky (above horizon)
|
||||
final skyPaint = Paint()..color = skyColor;
|
||||
final groundPaint = Paint()..color = groundColor;
|
||||
|
||||
// Create rotated horizon path
|
||||
canvas.save();
|
||||
canvas.translate(center.dx, center.dy);
|
||||
canvas.rotate(rollRad);
|
||||
canvas.translate(-center.dx, -center.dy);
|
||||
|
||||
// Sky rectangle (above horizon)
|
||||
canvas.drawRect(
|
||||
Rect.fromLTRB(
|
||||
center.dx - radius * 2,
|
||||
center.dy - radius * 2,
|
||||
center.dx + radius * 2,
|
||||
horizonY,
|
||||
),
|
||||
skyPaint,
|
||||
);
|
||||
|
||||
// Ground rectangle (below horizon)
|
||||
canvas.drawRect(
|
||||
Rect.fromLTRB(
|
||||
center.dx - radius * 2,
|
||||
horizonY,
|
||||
center.dx + radius * 2,
|
||||
center.dy + radius * 2,
|
||||
),
|
||||
groundPaint,
|
||||
);
|
||||
|
||||
// Horizon line
|
||||
final linePaint = Paint()
|
||||
..color = lineColor
|
||||
..strokeWidth = 2
|
||||
..style = PaintingStyle.stroke;
|
||||
|
||||
canvas.drawLine(
|
||||
Offset(center.dx - radius, horizonY),
|
||||
Offset(center.dx + radius, horizonY),
|
||||
linePaint,
|
||||
);
|
||||
|
||||
canvas.restore();
|
||||
|
||||
// Draw circle border
|
||||
final borderPaint = Paint()
|
||||
..color = lineColor.withValues(alpha: 0.5)
|
||||
..strokeWidth = borderWeight
|
||||
..style = PaintingStyle.stroke;
|
||||
|
||||
canvas.drawCircle(center, radius - 1, borderPaint);
|
||||
|
||||
// Draw center reference mark (fixed, doesn't rotate)
|
||||
final refPaint = Paint()
|
||||
..color = lineColor
|
||||
..strokeWidth = borderWeight * 0.8
|
||||
..style = PaintingStyle.stroke;
|
||||
|
||||
// Small wings
|
||||
canvas.drawLine(
|
||||
Offset(center.dx - radius * 0.3, center.dy),
|
||||
Offset(center.dx - radius * 0.1, center.dy),
|
||||
refPaint,
|
||||
);
|
||||
canvas.drawLine(
|
||||
Offset(center.dx + radius * 0.1, center.dy),
|
||||
Offset(center.dx + radius * 0.3, center.dy),
|
||||
refPaint,
|
||||
);
|
||||
// Center vertical line
|
||||
canvas.drawLine(
|
||||
Offset(center.dx, center.dy - radius * 0.05),
|
||||
Offset(center.dx, center.dy + radius * 0.1),
|
||||
refPaint,
|
||||
);
|
||||
|
||||
canvas.restore();
|
||||
}
|
||||
|
||||
@override
|
||||
bool shouldRepaint(_HorizonPainter oldDelegate) {
|
||||
return roll != oldDelegate.roll ||
|
||||
pitch != oldDelegate.pitch ||
|
||||
lineColor != oldDelegate.lineColor;
|
||||
}
|
||||
}
|
||||
@@ -17,6 +17,7 @@ SCRIPT_DIR = Path(__file__).parent.resolve()
|
||||
PROJECT_ROOT = SCRIPT_DIR.parent
|
||||
CONFIG_FILE = SCRIPT_DIR / "deploy_target.json"
|
||||
BACKEND_DIR = PROJECT_ROOT / "pi" / "backend"
|
||||
SERVICE_FILE = SCRIPT_DIR / "smartserow-backend.service"
|
||||
|
||||
|
||||
def run(cmd: list[str], check: bool = True, **kwargs) -> subprocess.CompletedProcess:
|
||||
@@ -72,6 +73,7 @@ def deploy(restart: bool = False) -> bool:
|
||||
"--exclude", "*.pyc",
|
||||
"--exclude", ".venv",
|
||||
"--exclude", ".ruff_cache",
|
||||
"--exclude", "uv.lock", # Let Pi generate its own lockfile
|
||||
f"{BACKEND_DIR}/",
|
||||
f"{ssh_target}:{remote_path}/",
|
||||
])
|
||||
@@ -88,6 +90,16 @@ def deploy(restart: bool = False) -> bool:
|
||||
print("WARNING: uv sync failed - dependencies may be out of date")
|
||||
print("Make sure uv is installed on Pi: curl -LsSf https://astral.sh/uv/install.sh | sh")
|
||||
|
||||
# Deploy service file if it exists
|
||||
if SERVICE_FILE.exists():
|
||||
print()
|
||||
print("Deploying systemd service file...")
|
||||
run(["scp", str(SERVICE_FILE), f"{ssh_target}:/tmp/"])
|
||||
run([
|
||||
"ssh", ssh_target,
|
||||
f"sudo mv /tmp/{SERVICE_FILE.name} /etc/systemd/system/ && sudo systemctl daemon-reload"
|
||||
])
|
||||
|
||||
# Restart service if requested
|
||||
if restart:
|
||||
print()
|
||||
|
||||
Reference in New Issue
Block a user