56 lines
1.6 KiB
C++
56 lines
1.6 KiB
C++
#include "fgc/TelemetryParser.h"
|
|
|
|
#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;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line) {
|
|
if (line.empty() || line[0] != '$') return std::nullopt;
|
|
|
|
std::vector<std::string> f = split(line, ';');
|
|
// Need indices 0..11: '$' marker + 11 data fields.
|
|
if (f.size() < 12) return std::nullopt;
|
|
|
|
try {
|
|
MotorTelemetry t;
|
|
t.encoder = std::stoi(f[1]);
|
|
t.encoder_err = std::stoi(f[2]);
|
|
t.sgt_val = std::stoi(f[3]);
|
|
t.sgt_stat = std::stoi(f[4]);
|
|
t.is_moving = std::stoi(f[5]);
|
|
t.control_status = std::stoi(f[6]);
|
|
t.heading = std::stof(f[7]);
|
|
t.deviation_warn = std::stoi(f[8]);
|
|
// NOTE: field order is humidity (index 9) BEFORE temperature (index 10),
|
|
// matching the controller firmware. See docs/known-issues.md #6 - do not
|
|
// swap without confirming the firmware output.
|
|
t.humidity = std::stoi(f[9]);
|
|
t.temperature = std::stoi(f[10]);
|
|
t.fan_pwm = std::stoi(f[11]);
|
|
return t;
|
|
} catch (const std::exception&) {
|
|
return std::nullopt;
|
|
}
|
|
}
|
|
|
|
} // namespace fgc
|