f1ed809c71
- null-terminated TSV frame: V_bat, IMU (9 fields), RPM, gear - mock RPM ramps 800-8000, gear derived from RPM bands - voltage calibration offset - PROTOCOL.md documents wire format Co-Authored-By: Claude Opus 4.5 <noreply@anthropic.com>
84 lines
2.1 KiB
C++
84 lines
2.1 KiB
C++
// Smart Serow - Main
|
|
// 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() {
|
|
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() {
|
|
// Always poll IMU - it's streaming at 20Hz
|
|
imu_update();
|
|
|
|
// Update mock RPM (ramping)
|
|
rpm_update();
|
|
|
|
// 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);
|
|
}
|