35 lines
1.0 KiB
Arduino
35 lines
1.0 KiB
Arduino
#include <Arduino.h>
|
|
#include "gps.h"
|
|
#include "imu.h"
|
|
#include "rs485_transport.h"
|
|
#include "sensor_node_config.h"
|
|
#include "calibration.h"
|
|
|
|
HardwareSerial gpsSerial(0), rs485Serial(1);
|
|
ImuState imuState;
|
|
GpsState gpsState;
|
|
unsigned long lastTelemetryMs = 0;
|
|
|
|
void setup() {
|
|
Serial.begin(115200); delay(250);
|
|
Serial.println("XIAO ESP32-C6 sensor node booting");
|
|
initGps(gpsSerial); initRs485(rs485Serial);
|
|
initLevelCalibration();
|
|
imuState.online = initImu();
|
|
Serial.println(imuState.online ? "BNO086 ready" : "BNO086 not detected");
|
|
}
|
|
|
|
void loop() {
|
|
updateGps(gpsSerial, gpsState); updateImu(imuState);
|
|
pollRs485Commands(rs485Serial, imuState);
|
|
String calibrationResult;
|
|
if (updateLevelCalibration(imuState, calibrationResult)) {
|
|
publishCalibrationResult(rs485Serial, calibrationResult);
|
|
}
|
|
unsigned long now = millis();
|
|
if (now - lastTelemetryMs >= TELEMETRY_INTERVAL_MS) {
|
|
publishSensorStatus(rs485Serial, imuState, gpsState); lastTelemetryMs = now;
|
|
}
|
|
delay(1);
|
|
}
|