145 lines
6.5 KiB
C++
145 lines
6.5 KiB
C++
#include <ArduinoJson.h>
|
|
#include <esp_system.h>
|
|
#include "rs485_transport.h"
|
|
#include "sensor_node_config.h"
|
|
#include "calibration.h"
|
|
|
|
namespace {
|
|
String commandBuffer;
|
|
bool discardCommandUntilNewline = false;
|
|
uint32_t bootId = 0;
|
|
uint32_t frameSequence = 0;
|
|
|
|
void drainTransmitEcho(HardwareSerial& serial) {
|
|
while (serial.available()) serial.read();
|
|
}
|
|
|
|
void sendJson(HardwareSerial& serial, JsonDocument& doc) {
|
|
if (doc.overflowed() || measureJson(doc) + 1 > RS485_MAX_FRAME_BYTES) {
|
|
#if USB_TELEMETRY_ENABLED
|
|
Serial.println("{\"type\":\"rs485_error\",\"error\":\"frame_too_large\"}");
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
digitalWrite(RS485_DE_PIN, HIGH);
|
|
delayMicroseconds(RS485_DRIVER_ENABLE_SETTLE_US);
|
|
serializeJson(doc, serial); serial.write('\n'); serial.flush();
|
|
// This expansion board can echo TX into RX. At flush completion the
|
|
// entire transmitted line has left the UART, so discard that local echo
|
|
// before returning the transceiver to receive mode.
|
|
drainTransmitEcho(serial);
|
|
digitalWrite(RS485_DE_PIN, LOW);
|
|
#if USB_TELEMETRY_ENABLED
|
|
serializeJson(doc, Serial); Serial.println();
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void initRs485(HardwareSerial& serial) {
|
|
pinMode(RS485_DE_PIN, OUTPUT); digitalWrite(RS485_DE_PIN, LOW);
|
|
serial.setRxBufferSize(RS485_RX_BUFFER_BYTES);
|
|
serial.begin(RS485_BAUD, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN);
|
|
bootId = esp_random();
|
|
}
|
|
|
|
void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsState& gps) {
|
|
StaticJsonDocument<2048> doc;
|
|
doc["type"] = "sensor_status"; doc["schema_version"] = SENSOR_NODE_SCHEMA_VERSION;
|
|
doc["node_id"] = SENSOR_NODE_NAME; doc["firmware_version"] = SENSOR_NODE_FIRMWARE_VERSION;
|
|
doc["boot_id"] = bootId;
|
|
doc["frame_sequence"] = frameSequence++;
|
|
doc["uptime_ms"] = millis();
|
|
JsonObject i = doc.createNestedObject("imu");
|
|
i["online"] = imu.online; i["valid"] = imu.valid && millis() - imu.lastUpdateMs <= SENSOR_STALE_MS;
|
|
i["accuracy"] = imu.accuracy;
|
|
JsonObject q = i.createNestedObject("quaternion"); q["i"] = imu.i; q["j"] = imu.j; q["k"] = imu.k; q["real"] = imu.real;
|
|
i["yaw_degrees"] = imu.yawDegrees; i["pitch_degrees"] = imu.pitchDegrees; i["roll_degrees"] = imu.rollDegrees;
|
|
i["age_ms"] = imu.lastUpdateMs ? millis() - imu.lastUpdateMs : -1;
|
|
auto addVector = [&](const char* name, const Vector3State& v) {
|
|
JsonObject o = i.createNestedObject(name);
|
|
o["x"] = v.x; o["y"] = v.y; o["z"] = v.z; o["accuracy"] = v.accuracy;
|
|
o["age_ms"] = v.lastUpdateMs ? millis() - v.lastUpdateMs : -1;
|
|
};
|
|
addVector("linear_acceleration_mps2", imu.linearAcceleration);
|
|
addVector("angular_velocity_rads", imu.angularVelocity);
|
|
addVector("gravity_mps2", imu.gravity);
|
|
i["stability"] = imu.stability; i["stability_code"] = imu.stabilityCode;
|
|
i["stability_age_ms"] = imu.stabilityLastUpdateMs ? millis() - imu.stabilityLastUpdateMs : -1;
|
|
JsonObject level = i.createNestedObject("level_calibration");
|
|
level["valid"] = levelCalibration.valid; level["active"] = levelCalibration.active;
|
|
level["pitch_offset_degrees"] = levelCalibration.pitchOffsetDegrees;
|
|
level["roll_offset_degrees"] = levelCalibration.rollOffsetDegrees;
|
|
level["pitch_degrees"] = calibratedPitchDegrees(imu);
|
|
level["roll_degrees"] = calibratedRollDegrees(imu);
|
|
JsonObject g = doc.createNestedObject("gps");
|
|
g["online"] = gps.online; g["fix"] = gps.fix; g["satellites"] = gps.satellites;
|
|
g["baud"] = gps.baud;
|
|
if (gps.hdopValid) g["hdop"] = gps.hdop; else g["hdop"] = nullptr;
|
|
g["location_age_ms"] = gps.locationAgeMs == ULONG_MAX ? -1 : (long)gps.locationAgeMs;
|
|
g["time_age_ms"] = gps.timeAgeMs == ULONG_MAX ? -1 : (long)gps.timeAgeMs;
|
|
JsonObject nmea = g.createNestedObject("nmea");
|
|
nmea["characters"] = gps.charactersProcessed;
|
|
nmea["sentences_passed"] = gps.sentencesPassed;
|
|
nmea["sentences_failed"] = gps.sentencesFailed;
|
|
g["latitude"] = gps.latitude; g["longitude"] = gps.longitude; g["altitude_meters"] = gps.altitudeMeters;
|
|
g["speed_kph"] = gps.speedKph; g["course_degrees"] = gps.courseDegrees;
|
|
JsonObject utc = g.createNestedObject("utc"); utc["year"] = gps.year; utc["month"] = gps.month; utc["day"] = gps.day;
|
|
utc["hour"] = gps.hour; utc["minute"] = gps.minute; utc["second"] = gps.second;
|
|
sendJson(serial, doc);
|
|
}
|
|
|
|
void publishCalibrationResult(HardwareSerial& serial, const String& result) {
|
|
StaticJsonDocument<384> doc;
|
|
doc["type"] = "calibration_response";
|
|
doc["calibration"] = "level";
|
|
doc["result"] = result;
|
|
doc["ok"] = result == "started" || result == "complete" || result == "cleared";
|
|
doc["valid"] = levelCalibration.valid;
|
|
doc["pitch_offset_degrees"] = levelCalibration.pitchOffsetDegrees;
|
|
doc["roll_offset_degrees"] = levelCalibration.rollOffsetDegrees;
|
|
sendJson(serial, doc);
|
|
}
|
|
|
|
void pollRs485Commands(HardwareSerial& serial, const ImuState& imu) {
|
|
// Bound RX work so an electrically echoed or noisy bus cannot starve sensors.
|
|
int bytesRemaining = RS485_COMMAND_MAX_LENGTH * 2;
|
|
while (serial.available() && bytesRemaining-- > 0) {
|
|
char c = serial.read();
|
|
if (c == '\r') continue;
|
|
if (c != '\n') {
|
|
if (discardCommandUntilNewline) continue;
|
|
if (commandBuffer.length() < RS485_COMMAND_MAX_LENGTH) {
|
|
commandBuffer += c;
|
|
} else {
|
|
commandBuffer = "";
|
|
discardCommandUntilNewline = true;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (discardCommandUntilNewline) {
|
|
discardCommandUntilNewline = false;
|
|
commandBuffer = "";
|
|
continue;
|
|
}
|
|
|
|
StaticJsonDocument<192> request;
|
|
DeserializationError error = deserializeJson(request, commandBuffer);
|
|
commandBuffer = "";
|
|
// Partial self-echoes and line noise are not commands. Dropping them
|
|
// silently avoids turning one bad fragment into a response/echo loop.
|
|
if (error) continue;
|
|
String type = request["type"] | "";
|
|
// Some RS485 boards echo transmitted frames into RX. Ignore our own output.
|
|
if (type == "sensor_status" || type == "calibration_response") continue;
|
|
if (type == "calibrate_level") {
|
|
publishCalibrationResult(serial, startLevelCalibration(imu) ? "started" : "imu_unavailable");
|
|
} else if (type == "clear_level_calibration") {
|
|
clearLevelCalibration(); publishCalibrationResult(serial, "cleared");
|
|
} else {
|
|
publishCalibrationResult(serial, "unknown_command");
|
|
}
|
|
}
|
|
}
|