GGS
/
fwt_software
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added extra encoder tracking information

This commit is contained in:
pgdalmeida 2026-06-29 23:35:45 +02:00
parent c972aba4d3
commit d4d1993a25
Signed by: pedro.almeida
GPG Key ID: D4A6C394DF13F1D7
12 changed files with 195 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config/config.example.ini
View File

@ -77,6 +77,10 @@ pitch_counts_per_deg = 8333.33
pitch_zero_count = 0
pitch_min_deg = -10
pitch_max_deg = 30
; Live encoder-tracking WARN: log a warning when an axis's |xactual - xenc|
; exceeds this many counts (positional slip). 0 disables. Default 400 sits above
; the firmware hold deadband (200) and below the DIAG peak limit (500).
enc_error_warn_counts = 400
[Scan]
; Capture scan grid (the (yaw,pitch) waypoints auto-sweep steps through).

1
include/fgc/Config.h
View File

@ -93,6 +93,7 @@ struct AppConfig {
UiConfig ui; // [UI] terminal dashboard toggle
Geometry geometry; // [Motor] degrees<->counts maps (yaw + pitch)
YawHomeZero yaw_home_zero = YawHomeZero::Off; // [Motor] re-anchor yaw zero on homing
long enc_error_warn_counts = 400; // [Motor] live encoder-error WARN (0=off)
ScanConfig scan; // [Scan] grid source
ImuConfig imu; // [IMU] Xsens MTi serial device

4
include/fgc/DumpParser.h
View File

@ -21,6 +21,10 @@ struct DumpAxis {
long soft_margin = 0; // counts kept clear of each hard limit
long hold_target = 0;
long speed = 0;
// Encoder-tracking health accumulated by the firmware hold corrector.
long hold_corrections = 0; // # of hold resyncs since firmware boot/RESET
long hold_peak_dev = 0; // largest deviation seen while holding (counts)
long hold_cumulative_dev = 0; // sum of corrected deviations (counts)
int hsub = 0;
// Raw register hex strings as received, keyed by name (GCONF, DRV_STATUS...).

3
include/fgc/mock/MockMotorController.h
View File

@ -58,7 +58,8 @@ public:
d << "DUMP " << L << " state=" << st
<< " hsub=0 enabled=" << (homed_ ? 1 : 0)
<< " lim_neg=0 lim_pos=1000000 hold_target=" << a.xenc
<< " speed=200000 eeprom_restored=1 soft_margin=5000 has_encoder=1\n";
<< " speed=200000 eeprom_restored=1 soft_margin=5000"
<< " hold_corr=0 hold_peak=0 hold_cumul=0 has_encoder=1\n";
d << "DUMP " << L << " TMC GCONF=0x00000004 GSTAT=0x00000000"
<< " IOIN=0x30000008 TSTEP=0x000fffff RAMPMODE=0x00000000"
<< " XACTUAL=0x" << std::hex << (a.xactual & 0xffffffff)

3
include/fgc/ui/TuiUi.h
View File

@ -47,6 +47,9 @@ private:
// When set, the 100ms auto-redraw is suspended so the screen holds still and
// the operator can select/copy text (terminals drop a selection on repaint).
std::atomic<bool> paused_{false};
// True while the gimbal overlay is open, so the refresh loop can periodically
// re-pull a firmware dump (keeps the tracking counters ticking without 'd').
std::atomic<bool> gimbal_overlay_open_{false};
// Log ring buffer (newest last), filled by the Logger sink.
std::mutex log_mutex_;

25
include/fgc/ui/UiSnapshot.h
View File

@ -23,6 +23,7 @@ struct AxisView {
double deg = 0.0; // heading/elevation, from xenc via Geometry
long xactual = 0;
long xenc = 0;
long enc_err = 0; // live tracking error = xactual - xenc (counts)
double target_deg = 0.0; // current scheduler target
int sg = 0; // SG_RESULT (live, from ST line)
int cs = 0; // CS_ACTUAL
@ -41,6 +42,12 @@ struct AxisView {
long lim_pos = 0;
double lim_neg_deg = 0.0;
double lim_pos_deg = 0.0;
// Encoder-tracking health (from the last firmware dump): disturbances the
// firmware hold corrector counted/erased. has_track gates rendering.
bool has_track = false;
long hold_corrections = 0;
long hold_peak_dev = 0;
long hold_cumulative_dev = 0;
};
struct GimbalView {
@ -197,6 +204,23 @@ struct CalibResultView {
CalibAxisView yaw, pitch;
};
// Worst-case encoder tracking error from the last DIAG run, per axis, for the
// gimbal expanded view. err_* are the largest values across that axis's tests
// (-1 when the axis has no encoder).
struct DiagAxisView {
char axis = '?';
bool has = false;
bool pass = false;
long err_peak = 0; // worst following error during the moves (counts)
long err_rms = 0; // worst RMS following error (counts)
long err_still = 0; // worst standstill error (counts)
};
struct DiagResultView {
bool has = false;
long long ts_ms = 0;
std::vector<DiagAxisView> axes; // Y and (if present) P
};
struct UiSnapshot {
HeaderView header;
GimbalView gimbal;
@ -208,6 +232,7 @@ struct UiSnapshot {
ImuView imu;
ActivityView activity;
CalibResultView calib;
DiagResultView diag;
};
// ---- Pure formatting helpers (unit-tested in tests/test_uisnapshot.cpp) ----

4
src/camera/ImagePipeline.cpp
View File

@ -78,6 +78,10 @@ void ImagePipeline::process(const Frame& frame) {
const fs::path dir = fs::path(params_.output_dir) / label;
std::error_code ec;
fs::create_directories(dir, ec);
if (ec) {
LOG_ERROR << "Cannot create capture dir " << dir.string() << ": " << ec.message();
return;
}
const fs::path file = dir / (std::to_string(frame.timestamp_ms) + ".jxl");
if (params_.demo) {

98
src/core/Application.cpp
View File

@ -24,12 +24,15 @@
#include "fgc/ui/IUserInterface.h"
#include "fgc/ui/UiSnapshot.h"
#include <algorithm>
#include <atomic>
#include <cctype>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <filesystem>
#include <fstream>
#include <memory>
#include <mutex>
#include <queue>
@ -57,6 +60,22 @@ long long nowEpochMs() {
.count();
}
// True if `dir` can be created and a probe file written there. A successful
// create_directories alone isn't enough (the dir may exist read-only), so we
// actually write+remove a probe file.
bool outputDirWritable(const std::string& dir) {
namespace fs = std::filesystem;
std::error_code ec;
fs::create_directories(dir, ec);
if (ec) return false;
const fs::path probe = fs::path(dir) / ".fgc_write_test";
std::ofstream f(probe);
if (!f.good()) return false;
f.close();
fs::remove(probe, ec);
return true;
}
// Human-readable list of the enabled wire-trace categories, for echoing back.
std::string traceNames(unsigned mask) {
if (mask == 0) return "none";
@ -164,6 +183,10 @@ struct Application::Impl {
std::vector<std::string> last_diag_summary;
long long last_diag_ts = 0; // epoch ms
bool last_diag_pass = false;
DiagResultView last_diag_view; // per-axis worst tracking error
// Rising-edge latches for the live encoder-error WARN (avoid 100Hz spam).
bool yaw_track_over_ = false;
bool pitch_track_over_ = false;
CalibResultView last_calib_view;
std::vector<std::string> last_calib_summary;
long long last_calib_ts = 0; // epoch ms
@ -265,6 +288,7 @@ struct Application::Impl {
v.deg = map.toDeg(a.xenc);
v.xactual = a.xactual;
v.xenc = a.xenc;
v.enc_err = a.xactual - a.xenc; // live tracking error (counts)
v.target_deg = map.toDeg(target_counts);
v.sg = a.sg;
v.cs = a.cs;
@ -292,6 +316,10 @@ struct Application::Impl {
v.lim_pos = a.lim_pos;
v.lim_neg_deg = map.toDeg(a.lim_neg);
v.lim_pos_deg = map.toDeg(a.lim_pos);
v.has_track = true;
v.hold_corrections = a.hold_corrections;
v.hold_peak_dev = a.hold_peak_dev;
v.hold_cumulative_dev = a.hold_cumulative_dev;
};
for (const auto& a : dd.axes) {
if (a.axis == 'Y') fillLimits(s.gimbal.yaw, a, cfg.geometry.yaw);
@ -389,6 +417,7 @@ struct Application::Impl {
// --- Activity strip + last calibration ---
s.calib = last_calib_view;
s.calib.pitch_present = s.gimbal.pitch_present || s.calib.pitch_present;
s.diag = last_diag_view;
fillActivity(s);
return s;
}
@ -463,6 +492,28 @@ struct Application::Impl {
latest_snapshot = std::move(s);
}
// Warn (once per excursion) when an axis's live encoder error |xactual-xenc|
// crosses the configured limit, so positional slip is visible in the log
// without watching the gimbal view. Hysteresis re-arms at 75% of the limit.
void checkTrackingError() {
const long limit = cfg.enc_error_warn_counts;
if (limit <= 0) return; // disabled
MotorTelemetry t = motor->telemetry();
auto check = [&](const char* axis, const AxisTelemetry& a, bool present, bool& over) {
if (!present) return;
const long err = std::labs(a.xactual - a.xenc);
if (!over && err > limit) {
over = true;
LOG_WARN << "encoder tracking error on " << axis << ": " << (a.xactual - a.xenc)
<< " counts (limit " << limit << ")";
} else if (over && err < limit * 3 / 4) {
over = false;
}
};
check("yaw", t.yaw, true, yaw_track_over_);
check("pitch", t.pitch, t.pitch_present, pitch_track_over_);
}
void runInitSequence() {
using namespace std::chrono_literals;
LOG_INFO << "Running gimbal init sequence (enable + home)";
@ -816,14 +867,35 @@ struct Application::Impl {
DiagResult dr = parseDiag(motor->lastDiag());
std::ostringstream summary;
for (const auto& l : formatDiag(dr)) { LOG_INFO << l; summary << l << "\n"; }
// Concise per-axis lines for the activity strip (full detail is logged).
// Concise per-axis lines for the activity strip + the per-axis worst
// tracking error for the gimbal view. Full detail is logged above.
last_diag_summary.clear();
for (const auto& ax : dr.axes)
last_diag_summary.push_back(std::string(1, ax.axis) + ": " +
(ax.pass ? "PASS" : "FAIL") + " (" +
std::to_string(ax.tests.size()) + " tests)");
last_diag_view = DiagResultView{};
last_diag_view.has = dr.valid;
for (const auto& ax : dr.axes) {
DiagAxisView dv;
dv.axis = ax.axis;
dv.has = !ax.tests.empty();
dv.pass = ax.pass;
dv.err_peak = dv.err_rms = dv.err_still = -1; // -1 stays if no encoder
for (const auto& tst : ax.tests) { // worst (max) across the tests
if (tst.err_peak < 0) continue; // no-encoder test: skip
dv.err_peak = std::max(dv.err_peak, tst.err_peak);
dv.err_rms = std::max(dv.err_rms, tst.err_rms);
dv.err_still = std::max(dv.err_still, tst.err_still);
}
last_diag_view.axes.push_back(dv);
std::string line = std::string(1, ax.axis) + ": " +
(ax.pass ? "PASS" : "FAIL");
if (dv.has && dv.err_peak >= 0) // -1 == no encoder
line += " peak " + std::to_string(dv.err_peak) +
" still " + std::to_string(dv.err_still);
line += " (" + std::to_string(ax.tests.size()) + " tests)";
last_diag_summary.push_back(line);
}
last_diag_pass = dr.allPass();
last_diag_ts = nowEpochMs();
last_diag_view.ts_ms = last_diag_ts;
std::string path = paths::writeLogFile(
paths::timestampedLogName("diag"),
motor->lastDiag() + "\n--- parsed ---\n" + summary.str());
@ -938,6 +1010,21 @@ struct Application::Impl {
if (!channel->connect())
LOG_WARN << "Control channel not connected; continuing in degraded mode";
// Verify the capture output dir is writable up front; if not (e.g. a
// misconfigured absolute path), fall back to the per-user default so
// captures land on disk instead of failing once per frame. Resolved once
// here, then used everywhere via cfg.paths.output_dir.
if (!outputDirWritable(cfg.paths.output_dir)) {
const std::string fb = paths::defaultOutputDir();
LOG_ERROR << "capture output_dir '" << cfg.paths.output_dir
<< "' is not writable; falling back to '" << fb << "'";
cfg.paths.output_dir = fb;
if (!outputDirWritable(cfg.paths.output_dir))
LOG_ERROR << "fallback output_dir '" << fb
<< "' is also not writable; captures will fail to save";
}
LOG_INFO << "capture output dir: " << cfg.paths.output_dir;
ImagePipeline::Params pp;
pp.output_dir = cfg.paths.output_dir;
pp.labels = cfg.camera.labels;
@ -1006,6 +1093,7 @@ struct Application::Impl {
drainCommands();
pollBackgroundResults(); // apply calibration result / emit DIAG summary+log
scheduler->tick();
checkTrackingError(); // warn on live encoder-error excursions
publishSnapshot();
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

2
src/core/Config.cpp
View File

@ -140,6 +140,8 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
: hz == "low" ? YawHomeZero::Low
: YawHomeZero::Off;
}
cfg.enc_error_warn_counts = getLong(kv, "Motor.enc_error_warn_counts",
cfg.enc_error_warn_counts);
// [Scan]: scan-grid source (explicit CSV file, else generated).
cfg.scan.grid_file = get(kv, "Scan.grid_file", cfg.scan.grid_file);

3
src/core/DumpParser.cpp
View File

@ -162,6 +162,9 @@ DumpData parseDump(const std::string& block) {
ax.soft_margin = asInt(get(m, "soft_margin", "0"));
ax.hold_target = asInt(get(m, "hold_target", "0"));
ax.speed = asInt(get(m, "speed", "0"));
ax.hold_corrections = asInt(get(m, "hold_corr", "0"));
ax.hold_peak_dev = asInt(get(m, "hold_peak", "0"));
ax.hold_cumulative_dev = asInt(get(m, "hold_cumul", "0"));
}
}

53
src/ui/TuiUi.cpp
View File

@ -321,6 +321,7 @@ Element axisLiveDetail(const AxisView& a) {
text(formatDegrees(a.deg) + " -> " + formatDegrees(a.target_deg)) | bold}),
kvRow("xactual", std::to_string(a.xactual)),
kvRow("xenc", std::to_string(a.xenc)),
kvRow("enc err", std::to_string(a.enc_err)), // live tracking error xactual-xenc
kvRow("SG_RESULT", std::to_string(a.sg)),
kvRow("CS_ACTUAL", std::to_string(a.cs)),
kvRow("PWM", std::to_string(a.pwm)),
@ -384,7 +385,8 @@ Element axisDumpDetail(const DumpAxis& ax) {
// registers. Rendered per axis so the two axes sit side by side and every row
// (incl. RAMP_STAT, the last one) is visible without scrolling.
Element axisColumn(const std::string& title, const AxisView& live, const DumpData& d,
char letter, bool calib_has, const CalibAxisView& cal) {
char letter, bool calib_has, const CalibAxisView& cal,
const DiagAxisView* diag) {
auto fmt = [](const char* f, double v) {
char b[32];
std::snprintf(b, sizeof(b), f, v);
@ -404,6 +406,31 @@ Element axisColumn(const std::string& title, const AxisView& live, const DumpDat
col.push_back(text(d.valid ? "(axis absent from dump)"
: "(awaiting dump - press 'd')") | dim);
// Encoder-tracking health: disturbances the firmware hold corrector counted
// and erased (from the firmware dump), plus the last DIAG tracking errors.
col.push_back(separator());
col.push_back(text("TRACKING") | bold | color(Color::Yellow));
if (live.has_track) {
col.push_back(kvRow("corrections", std::to_string(live.hold_corrections)));
col.push_back(kvRow("peak dev", std::to_string(live.hold_peak_dev)));
col.push_back(kvRow("cumul slip", std::to_string(live.hold_cumulative_dev)));
} else {
col.push_back(text("(awaiting dump - press 'd')") | dim);
}
if (diag && diag->has) {
if (diag->err_peak < 0) {
col.push_back(kvRow("diag", "no encoder"));
} else {
col.push_back(kvRow("diag peak", std::to_string(diag->err_peak)));
col.push_back(kvRow("diag rms", std::to_string(diag->err_rms)));
col.push_back(kvRow("diag still", std::to_string(diag->err_still)));
col.push_back(kvRow("diag", diag->pass ? "PASS" : "FAIL",
diag->pass ? Color::Green : Color::Red));
}
} else {
col.push_back(kvRow("diag", "run 'diag'", Color::GrayDark));
}
// Last calibration result for this axis.
col.push_back(separator());
col.push_back(text("CALIBRATION") | bold | color(Color::Magenta));
@ -421,8 +448,14 @@ Element axisColumn(const std::string& title, const AxisView& live, const DumpDat
// Full-screen gimbal view (toggled with 'g'): one column per axis, each with
// live telemetry above its decoded firmware register dump + last calibration.
Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump, const CalibResultView& calib) {
Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump, const CalibResultView& calib,
const DiagResultView& diag) {
DumpData d = parseDump(dump.text);
auto diagFor = [&](char axis) -> const DiagAxisView* {
for (const auto& a : diag.axes)
if (a.axis == axis) return &a;
return nullptr;
};
std::string reset;
for (size_t i = 0; i < d.reset_flags.size(); ++i)
@ -444,8 +477,9 @@ Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump, const Calib
});
std::vector<Element> cols;
cols.push_back(axisColumn("YAW", g.yaw, d, 'Y', calib.has, calib.yaw));
if (g.pitch_present) cols.push_back(axisColumn("PITCH", g.pitch, d, 'P', calib.has, calib.pitch));
cols.push_back(axisColumn("YAW", g.yaw, d, 'Y', calib.has, calib.yaw, diagFor('Y')));
if (g.pitch_present)
cols.push_back(axisColumn("PITCH", g.pitch, d, 'P', calib.has, calib.pitch, diagFor('P')));
return window(text(" GIMBAL (g/Esc:close d:refresh dump) ") | bold | color(Color::Cyan),
vbox({header, separator(), hbox(std::move(cols)) | flex}));
@ -584,10 +618,18 @@ void TuiUi::pushLog(LogLevel level, const std::string& line) {
void TuiUi::refreshLoop() {
using namespace std::chrono_literals;
auto last_dump = std::chrono::steady_clock::now();
while (running_) {
std::this_thread::sleep_for(100ms);
if (!running_) break;
if (paused_) continue; // frozen for text selection; don't repaint
// While the gimbal overlay is open, re-pull a firmware dump every ~3s so
// the encoder-tracking counters stay current without pressing 'd'.
if (gimbal_overlay_open_.load() && sink_ &&
std::chrono::steady_clock::now() - last_dump > 3s) {
sink_("gimbal dump");
last_dump = std::chrono::steady_clock::now();
}
if (auto* s = screen_.load()) s->PostEvent(Event::Custom); // force a redraw
}
}
@ -610,6 +652,7 @@ void TuiUi::uiLoop() {
s.log.assign(log_.begin(), log_.end());
}
calib_prompt = !s.activity.prompt.empty();
gimbal_overlay_open_.store(overlay == Overlay::Gimbal); // refreshLoop polls a dump while open
std::string mode = s.header.live ? "LIVE" : "MOCK";
Element header = hbox({
@ -647,7 +690,7 @@ void TuiUi::uiLoop() {
return vbox({header, separator(), helpPanel(help_sel, s.dump) | flex, bottom});
case Overlay::Gimbal:
return vbox({header, separator(),
gimbalDetailPanel(s.gimbal, s.dump, s.calib) | flex, bottom});
gimbalDetailPanel(s.gimbal, s.dump, s.calib, s.diag) | flex, bottom});
case Overlay::Sensors:
return vbox({header, separator(), imuDetailPanel(s.imu) | flex, bottom});
case Overlay::Cameras:

7
tests/test_dumpparser.cpp
View File

@ -15,7 +15,8 @@ namespace {
const char* kDump =
"DUMP BEGIN build=11dd3ce-dirty uptime=11740067 mcusr=0x01 free_ram=1852\n"
"DUMP Y state=3 hsub=10 enabled=1 lim_neg=-82919 lim_pos=98687 hold_target=-90 "
"speed=50000 eeprom_restored=0 has_encoder=1\n"
"speed=50000 eeprom_restored=0 soft_margin=5000 hold_corr=7 hold_peak=430 hold_cumul=2810 "
"has_encoder=1\n"
"DUMP Y TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
"RAMPMODE=0x00000000 XACTUAL=0xFFFFFFA6 VACTUAL=0x00000000 XTARGET=0xFFFFFFA6 "
"SW_MODE=0x000008A0 RAMP_STAT=0x00001680 X_ENC=0xFFFFFF7F ENC_STATUS=0x00000002 "
@ -64,6 +65,10 @@ TEST_CASE("parseDump decodes the per-axis state line") {
CHECK(y->hold_target == -90);
CHECK(y->speed == 50000);
CHECK(y->hsub == 10);
CHECK(y->soft_margin == 5000);
CHECK(y->hold_corrections == 7);
CHECK(y->hold_peak_dev == 430);
CHECK(y->hold_cumulative_dev == 2810);
const DumpAxis* p = axis(d, 'P');
REQUIRE(p != nullptr);