diff --git a/config/config.example.ini b/config/config.example.ini index dfcf592..f4e9199 100644 --- a/config/config.example.ini +++ b/config/config.example.ini @@ -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). diff --git a/include/fgc/Config.h b/include/fgc/Config.h index f7b68e6..fde5515 100644 --- a/include/fgc/Config.h +++ b/include/fgc/Config.h @@ -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 diff --git a/include/fgc/DumpParser.h b/include/fgc/DumpParser.h index 75ed950..c388e72 100644 --- a/include/fgc/DumpParser.h +++ b/include/fgc/DumpParser.h @@ -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...). diff --git a/include/fgc/mock/MockMotorController.h b/include/fgc/mock/MockMotorController.h index 797da9a..dc0c040 100644 --- a/include/fgc/mock/MockMotorController.h +++ b/include/fgc/mock/MockMotorController.h @@ -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) diff --git a/include/fgc/ui/TuiUi.h b/include/fgc/ui/TuiUi.h index 988dfa5..432706a 100644 --- a/include/fgc/ui/TuiUi.h +++ b/include/fgc/ui/TuiUi.h @@ -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 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 gimbal_overlay_open_{false}; // Log ring buffer (newest last), filled by the Logger sink. std::mutex log_mutex_; diff --git a/include/fgc/ui/UiSnapshot.h b/include/fgc/ui/UiSnapshot.h index 2859f10..97e27d6 100644 --- a/include/fgc/ui/UiSnapshot.h +++ b/include/fgc/ui/UiSnapshot.h @@ -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 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) ---- diff --git a/src/camera/ImagePipeline.cpp b/src/camera/ImagePipeline.cpp index 128dba5..1af028b 100644 --- a/src/camera/ImagePipeline.cpp +++ b/src/camera/ImagePipeline.cpp @@ -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) { diff --git a/src/core/Application.cpp b/src/core/Application.cpp index e93f704..6c61a6d 100644 --- a/src/core/Application.cpp +++ b/src/core/Application.cpp @@ -24,12 +24,15 @@ #include "fgc/ui/IUserInterface.h" #include "fgc/ui/UiSnapshot.h" +#include #include #include #include #include #include #include +#include +#include #include #include #include @@ -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 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 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)); } diff --git a/src/core/Config.cpp b/src/core/Config.cpp index 662821c..3c52dd1 100644 --- a/src/core/Config.cpp +++ b/src/core/Config.cpp @@ -140,6 +140,8 @@ AppConfig ConfigLoader::fromMap(const std::map& 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); diff --git a/src/core/DumpParser.cpp b/src/core/DumpParser.cpp index 94dcff7..113dea1 100644 --- a/src/core/DumpParser.cpp +++ b/src/core/DumpParser.cpp @@ -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")); } } diff --git a/src/ui/TuiUi.cpp b/src/ui/TuiUi.cpp index a2ad7bc..04c1c7b 100644 --- a/src/ui/TuiUi.cpp +++ b/src/ui/TuiUi.cpp @@ -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 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: diff --git a/tests/test_dumpparser.cpp b/tests/test_dumpparser.cpp index 28ba1e1..ebd6d70 100644 --- a/tests/test_dumpparser.cpp +++ b/tests/test_dumpparser.cpp @@ -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);