97 lines
2.9 KiB
C++
97 lines
2.9 KiB
C++
#include "fgc/TelemetryParser.h"
|
|
|
|
#include <sstream>
|
|
#include <vector>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
std::vector<std::string> split(const std::string& s, char delim) {
|
|
std::vector<std::string> out;
|
|
std::string cur;
|
|
for (char c : s) {
|
|
if (c == delim) {
|
|
out.push_back(cur);
|
|
cur.clear();
|
|
} else if (c != '\r' && c != '\n') {
|
|
cur += c;
|
|
}
|
|
}
|
|
out.push_back(cur);
|
|
return out;
|
|
}
|
|
|
|
AxisState stateFromChar(char c) {
|
|
switch (c) {
|
|
case 'B': case 'b': return AxisState::Boot;
|
|
case 'R': case 'r': return AxisState::Reset;
|
|
case 'H': case 'h': return AxisState::Homing;
|
|
case 'A': case 'a': return AxisState::Ready;
|
|
case 'E': case 'e': return AxisState::Error;
|
|
default: return AxisState::Unknown;
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
std::optional<AxisTelemetry> parseAxisSegment(const std::string& seg) {
|
|
// seg looks like "A,982,969,80084000,0,8,8,Se" (the part after "Y:").
|
|
std::vector<std::string> f = split(seg, ',');
|
|
if (f.size() < 7 || f[0].empty()) return std::nullopt; // flags field (index 7) is optional
|
|
try {
|
|
AxisTelemetry a;
|
|
a.state = stateFromChar(f[0][0]);
|
|
a.xactual = std::stol(f[1]);
|
|
a.xenc = std::stol(f[2]);
|
|
a.drv_status = static_cast<unsigned>(std::stoul(f[3], nullptr, 16));
|
|
a.sg = std::stoi(f[4]);
|
|
a.cs = std::stoi(f[5]);
|
|
a.pwm = std::stoi(f[6]);
|
|
if (f.size() > 7) {
|
|
for (char c : f[7]) {
|
|
switch (c) {
|
|
case 'S': a.standstill = true; break;
|
|
case 's': a.stall = true; break;
|
|
case 'o': case 'O': a.overtemp = true; break;
|
|
case 'L': a.endstop_l = true; break;
|
|
case 'R': a.endstop_r = true; break;
|
|
default: break; // 'e' (eeprom) and unknown letters ignored
|
|
}
|
|
}
|
|
}
|
|
return a;
|
|
} catch (const std::exception&) {
|
|
return std::nullopt;
|
|
}
|
|
}
|
|
|
|
std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line) {
|
|
// Expect: ST Y:<...> [P:<...>]
|
|
std::istringstream in(line);
|
|
std::string tok;
|
|
if (!(in >> tok) || tok != "ST") return std::nullopt;
|
|
|
|
MotorTelemetry t;
|
|
bool saw_yaw = false;
|
|
while (in >> tok) {
|
|
// tok is "Y:..." or "P:..."
|
|
auto colon = tok.find(':');
|
|
if (colon == std::string::npos) return std::nullopt;
|
|
char axis = tok[0];
|
|
auto seg = parseAxisSegment(tok.substr(colon + 1));
|
|
if (!seg) return std::nullopt;
|
|
if (axis == 'Y' || axis == 'y') {
|
|
t.yaw = *seg;
|
|
saw_yaw = true;
|
|
} else if (axis == 'P' || axis == 'p') {
|
|
t.pitch = *seg;
|
|
t.pitch_present = true;
|
|
}
|
|
}
|
|
if (!saw_yaw) return std::nullopt;
|
|
return t;
|
|
}
|
|
|
|
} // namespace fgc
|