nick/overland-controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

firmware: add persistent level calibration

Browse Source
This commit is contained in:
nick
2026-07-17 02:37:49 -06:00
parent fb2ca464b9
commit 4f96902068
9 changed files with 212 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp
+56 -7
View File
@@ -1,6 +1,19 @@
#include <ArduinoJson.h>
#include "rs485_transport.h"
#include "sensor_node_config.h"
#include "calibration.h"
namespace {
String commandBuffer;
void sendJson(HardwareSerial& serial, JsonDocument& doc) {
digitalWrite(RS485_DE_PIN, HIGH); delayMicroseconds(20);
serializeJson(doc, serial); serial.write('\n'); serial.flush();
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);
@@ -17,6 +30,12 @@ void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsS
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;
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;
@@ -28,12 +47,42 @@ void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsS
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;
digitalWrite(RS485_DE_PIN, HIGH); delayMicroseconds(20);
serializeJson(doc, serial); serial.write('\n'); serial.flush();
digitalWrite(RS485_DE_PIN, LOW);
sendJson(serial, doc);
}
#if USB_TELEMETRY_ENABLED
serializeJson(doc, Serial);
Serial.println();
#endif
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) {
while (serial.available()) {
char c = serial.read();
if (c == '\r') continue;
if (c != '\n') {
if (commandBuffer.length() < RS485_COMMAND_MAX_LENGTH) commandBuffer += c;
else commandBuffer = "";
continue;
}
StaticJsonDocument<192> request;
DeserializationError error = deserializeJson(request, commandBuffer);
commandBuffer = "";
if (error) { publishCalibrationResult(serial, "invalid_json"); continue; }
String type = request["type"] | "";
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");
}
}
}