#include "fgc/TestRunner.h" #include "fgc/Calibration.h" // unwrapNear #include "fgc/DiagParser.h" #include "fgc/AxisSpeed.h" #include "fgc/HardwareChecks.h" #include "fgc/HardwarePresence.h" #include "fgc/ICameraSource.h" #include "fgc/IEnvSensor.h" #include "fgc/TmcCurrent.h" #include "fgc/sensors/I2cBus.h" #include "fgc/DumpParser.h" #include "fgc/HostMetrics.h" #include "fgc/IImuSource.h" #include "fgc/IMotorController.h" #include "fgc/Logger.h" #include #include #include #include #include #include #include #include #include // gethostname namespace fgc { using namespace std::chrono_literals; using clock_t_ = std::chrono::steady_clock; namespace { long long nowEpochMs() { return std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()) .count(); } std::string hostName() { char buf[256] = {0}; if (gethostname(buf, sizeof(buf) - 1) == 0) return buf; return "unknown"; } // ---- small stats over a sample vector ---- struct Stats { double mean = 0, sd = 0, min = 0, max = 0, spread = 0; int n = 0; }; Stats stats(const std::vector& v) { Stats s; s.n = static_cast(v.size()); if (v.empty()) return s; s.min = s.max = v.front(); double sum = 0; for (double x : v) { sum += x; s.min = std::min(s.min, x); s.max = std::max(s.max, x); } s.mean = sum / v.size(); double acc = 0; for (double x : v) acc += (x - s.mean) * (x - s.mean); s.sd = v.size() > 1 ? std::sqrt(acc / (v.size() - 1)) : 0.0; s.spread = s.max - s.min; return s; } // ---- metric builders ---- TestMetric& add(TestSection& s, const std::string& name, double value, const std::string& unit, bool lower_is_better = true) { s.metrics.push_back(TestMetric{}); TestMetric& m = s.metrics.back(); m.name = name; m.value = value; m.unit = unit; m.lower_is_better = lower_is_better; return m; } void thresh(TestMetric& m, double t) { m.has_threshold = true; m.threshold = t; m.pass = m.lower_is_better ? (m.value <= t) : (m.value >= t); } void rollup(TestSection& s) { s.ran = true; s.pass = true; for (const auto& m : s.metrics) if (!m.pass) s.pass = false; } } // namespace bool resolveTestSelection(const std::string& subsystem, const std::string& test, uint32_t& mask, std::string& err) { auto leaf = [&](const char* s, const char* t, uint32_t bit, uint32_t& m) -> bool { if (subsystem == s && (test.empty() || test == t)) { m |= bit; return true; } return false; }; if (subsystem.empty()) { mask = T_ALL; return true; } uint32_t m = 0; bool sub_ok = false; if (subsystem == "gimbal") { sub_ok = true; if (test.empty()) m = T_GIMBAL_ALL; else if (test == "homing") m = T_GIMBAL_HOMING; else if (test == "encoder") m = T_GIMBAL_ENCODER; else if (test == "friction") m = T_GIMBAL_FRICTION; else if (test == "backlash") m = T_GIMBAL_BACKLASH; else if (test == "balance") m = T_GIMBAL_BALANCE; else { err = "unknown gimbal test: " + test; return false; } } else if (subsystem == "imu") { sub_ok = true; if (test.empty()) m = T_IMU_ALL; else if (test == "config") m = T_IMU_CONFIG; else if (test == "health") m = T_IMU_HEALTH; else if (test == "drift") m = T_IMU_DRIFT; else { err = "unknown imu test: " + test; return false; } } else if (subsystem == "hardware") { sub_ok = true; if (test.empty()) m = T_HW_ALL; else if (test == "axes") m = T_HW_AXES; else if (test == "motor") m = T_HW_MOTOR; else if (test == "imu") m = T_HW_IMU; else if (test == "env") m = T_HW_ENV; else if (test == "camera") m = T_HW_CAMERA; else { err = "unknown hardware test: " + test; return false; } } else if (subsystem == "host") { sub_ok = true; if (test.empty()) m = T_HOST_ALL; else if (test == "thermal") m = T_HOST_THERMAL; else if (test == "disk") m = T_HOST_DISK; else if (test == "memory") m = T_HOST_MEMORY; else if (test == "load") m = T_HOST_LOAD; else { err = "unknown host test: " + test; return false; } } (void)leaf; if (!sub_ok) { err = "unknown test subsystem: " + subsystem; return false; } mask = m; return true; } TestRunner::TestRunner(IMotorController& motor, IImuSource* imu, Geometry geo, uint32_t selection, TestProfile profile, std::string disk_path, HardwareTestContext hw) : motor_(motor), imu_(imu), geo_(geo), selection_(selection), profile_(std::move(profile)), disk_path_(std::move(disk_path)), hw_(std::move(hw)) {} TestRunner::~TestRunner() { cancel_ = true; if (thread_.joinable()) thread_.join(); } bool TestRunner::start() { if (running_.exchange(true)) { LOG_WARN << "test already running"; return false; } cancel_ = false; { std::lock_guard lk(result_mutex_); report_.reset(); } thread_ = std::thread([this] { run(); }); return true; } void TestRunner::cancel() { cancel_ = true; } std::optional TestRunner::takeReport() { std::lock_guard lk(result_mutex_); if (!report_) return std::nullopt; auto out = std::move(report_); report_.reset(); return out; } TestProgress TestRunner::progress() const { std::lock_guard lk(progress_mutex_); return progress_; } void TestRunner::setProgress(const std::string& section, int step, int total, const char* phase) { std::lock_guard lk(progress_mutex_); progress_.running = true; progress_.section = section; progress_.step = step; progress_.total = total; progress_.phase = phase; } void TestRunner::run() { struct Done { TestRunner* self; ~Done() { std::lock_guard lk(self->progress_mutex_); self->progress_ = TestProgress{}; self->running_ = false; } } done{this}; TestReport r; r.ts_ms = nowEpochMs(); r.profile = profile_.name.empty() ? "default" : profile_.name; r.host = hostName(); r.fw = ""; // populated from a DUMP build string if available // A homing run also surfaces the firmware build id for the report header. if (!cancelled() && (selection_ & T_GIMBAL_HOMING)) testHoming(r); if (!cancelled() && (selection_ & T_GIMBAL_ENCODER)) testEncoder(r); if (!cancelled() && (selection_ & T_GIMBAL_FRICTION)) testFriction(r); if (!cancelled() && (selection_ & T_GIMBAL_BACKLASH)) testBacklash(r); if (!cancelled() && (selection_ & T_GIMBAL_BALANCE)) testBalance(r); if (!cancelled() && (selection_ & T_IMU_CONFIG)) testImuConfig(r); if (!cancelled() && (selection_ & T_IMU_HEALTH)) testImuHealth(r); if (!cancelled() && (selection_ & T_IMU_DRIFT)) testImuDrift(r); if (!cancelled() && (selection_ & T_HOST_ALL)) testHost(r); if (!cancelled() && (selection_ & T_HW_AXES)) testHwAxes(r); if (!cancelled() && (selection_ & T_HW_MOTOR)) testHwMotor(r); if (!cancelled() && (selection_ & T_HW_IMU)) testHwImu(r); if (!cancelled() && (selection_ & T_HW_ENV)) testHwEnv(r); if (!cancelled() && (selection_ & T_HW_CAMERA)) testHwCamera(r); r.all_pass = true; for (const auto& s : r.sections) if (s.ran && !s.pass) r.all_pass = false; { std::lock_guard lk(result_mutex_); report_ = std::move(r); } if (cancelled()) LOG_WARN << "test cancelled"; } // --------------------------------------------------------------------------- // Motor helpers // --------------------------------------------------------------------------- namespace { // Poll telemetry until `pred` is true or `timeout` elapses. Returns elapsed ms, // or -1 on timeout/cancel. Calls `tick` each poll for sampling. template double pollUntil(IMotorController& motor, const std::atomic& cancel, Pred pred, Tick tick, std::chrono::milliseconds timeout, std::chrono::milliseconds period = 20ms) { const auto t0 = clock_t_::now(); const auto deadline = t0 + timeout; while (clock_t_::now() < deadline) { if (cancel.load()) return -1; MotorTelemetry t = motor.telemetry(); tick(t); if (pred(t)) return std::chrono::duration(clock_t_::now() - t0).count(); std::this_thread::sleep_for(period); } return -1; } bool axisReady(const MotorTelemetry& t) { return t.yaw.ready() && (!t.pitch_present || t.pitch.ready()); } } // namespace void TestRunner::testHoming(TestReport& r) { TestSection s; s.id = "homing"; s.title = "homing reproducibility"; const auto t_start = clock_t_::now(); const int reps = std::max(2, profile_.geti("reps", 5)); const auto home_to = std::chrono::milliseconds(profile_.geti("home_timeout_ms", 65000)); motor_.sendCommand("ENABLE Y"); motor_.sendCommand("ENABLE P"); std::vector yaw_neg, yaw_pos, pit_neg, pit_pos, durations; int failures = 0; bool pitch_present = false; for (int i = 0; i < reps && !cancelled(); ++i) { setProgress("homing", i + 1, reps, "homing"); motor_.sendCommand("HOME"); // wait for homing to begin (axis leaves READY), tolerating EEPROM fast-path pollUntil(motor_, cancel_, [](const MotorTelemetry& t) { return !axisReady(t); }, [](const MotorTelemetry&) {}, std::chrono::milliseconds(profile_.geti("home_begin_timeout_ms", 3000))); double dur = pollUntil( motor_, cancel_, [](const MotorTelemetry& t) { if (t.yaw.state == AxisState::Error || (t.pitch_present && t.pitch.state == AxisState::Error)) return true; return axisReady(t); }, [](const MotorTelemetry&) {}, home_to); MotorTelemetry t = motor_.telemetry(); if (dur < 0 || t.yaw.state == AxisState::Error || (t.pitch_present && t.pitch.state == AxisState::Error)) { ++failures; continue; } durations.push_back(dur); // Read the homed endstop limits from a fresh DUMP. motor_.sendCommand("DUMP"); std::this_thread::sleep_for(150ms); DumpData d = parseDump(motor_.lastDump()); if (r.fw.empty() && !d.build.empty()) r.fw = d.build; for (const auto& ax : d.axes) { if (ax.axis == 'Y') { yaw_neg.push_back(ax.lim_neg); yaw_pos.push_back(ax.lim_pos); } else if (ax.axis == 'P') { pitch_present = true; pit_neg.push_back(ax.lim_neg); pit_pos.push_back(ax.lim_pos); } } } const double spread_max = profile_.get("home_spread_max_counts", 80); if (!yaw_neg.empty()) { thresh(add(s, "yaw_lim_neg_spread", stats(yaw_neg).spread, "counts"), spread_max); thresh(add(s, "yaw_lim_pos_spread", stats(yaw_pos).spread, "counts"), spread_max); } if (pitch_present) { thresh(add(s, "pitch_lim_neg_spread", stats(pit_neg).spread, "counts"), spread_max); thresh(add(s, "pitch_lim_pos_spread", stats(pit_pos).spread, "counts"), spread_max); } if (!durations.empty()) { Stats ds = stats(durations); add(s, "home_ms_mean", ds.mean, "ms"); add(s, "home_ms_max", ds.max, "ms"); auto& tm = add(s, "home_ms_spread", ds.spread, "ms"); if (profile_.get("home_ms_max", 0) > 0) thresh(tm, profile_.get("home_ms_max", 0) - profile_.get("home_ms_min", 0)); } thresh(add(s, "homing_failures", failures, "count"), 0); if (yaw_neg.empty() && failures == 0) s.notes.push_back("no DUMP limits parsed (mock or no encoder)"); s.duration_ms = std::chrono::duration(clock_t_::now() - t_start).count(); rollup(s); r.sections.push_back(std::move(s)); } void TestRunner::testEncoder(TestReport& r) { TestSection s; s.id = "encoder"; s.title = "encoder tracking + repeatability"; const auto t_start = clock_t_::now(); // (a) firmware DIAG tracking quality, repeated for spread. const int reps = std::max(1, profile_.geti("encoder_diag_reps", 2)); std::vector y_peak, y_still, p_peak, p_still; bool any_diag = false; for (int i = 0; i < reps && !cancelled(); ++i) { setProgress("encoder", i + 1, reps, "diag"); unsigned seq0 = motor_.diagSeq(); motor_.sendCommand("DIAG"); // wait for a fresh diag completion const auto deadline = clock_t_::now() + 60s; while (clock_t_::now() < deadline && !cancelled() && motor_.diagSeq() == seq0) std::this_thread::sleep_for(50ms); if (motor_.diagSeq() == seq0) { s.notes.push_back("DIAG did not complete"); continue; } any_diag = true; DiagResult dr = parseDiag(motor_.lastDiag()); for (const auto& ax : dr.axes) { long peak = -1, still = -1; for (const auto& tst : ax.tests) { if (tst.err_peak < 0) continue; peak = std::max(peak, tst.err_peak); still = std::max(still, tst.err_still); } if (peak < 0) continue; if (ax.axis == 'Y') { y_peak.push_back(peak); y_still.push_back(still); } else if (ax.axis == 'P') { p_peak.push_back(peak); p_still.push_back(still); } } } const double err_peak_max = profile_.get("encoder_err_peak_max_counts", 0); auto emit = [&](const char* ax, std::vector& peak, std::vector& still) { if (peak.empty()) return; auto& mp = add(s, std::string(ax) + "_err_peak", stats(peak).max, "counts"); if (err_peak_max > 0) thresh(mp, err_peak_max); add(s, std::string(ax) + "_err_peak_spread", stats(peak).spread, "counts"); add(s, std::string(ax) + "_err_still", stats(still).max, "counts"); }; emit("yaw", y_peak, y_still); emit("pitch", p_peak, p_still); if (!any_diag) s.notes.push_back("no DIAG data captured"); // (c) hold integrity: hold-corrector delta across a short window. motor_.sendCommand("DUMP"); std::this_thread::sleep_for(150ms); DumpData d0 = parseDump(motor_.lastDump()); std::this_thread::sleep_for(std::chrono::milliseconds(profile_.geti("hold_window_ms", 2000))); if (cancelled()) { s.duration_ms = 0; rollup(s); r.sections.push_back(std::move(s)); return; } motor_.sendCommand("DUMP"); std::this_thread::sleep_for(150ms); DumpData d1 = parseDump(motor_.lastDump()); auto holdOf = [](const DumpData& d, char ax) -> long { for (const auto& a : d.axes) if (a.axis == ax) return a.hold_corrections; return 0; }; if (d0.valid && d1.valid) { thresh(add(s, "yaw_hold_corrections", holdOf(d1, 'Y') - holdOf(d0, 'Y'), "count"), profile_.get("hold_corrections_max", 0)); } s.duration_ms = std::chrono::duration(clock_t_::now() - t_start).count(); rollup(s); r.sections.push_back(std::move(s)); } // Sample motor load (cs/pwm peak, sg min) and time a move to a target. Returns // elapsed ms (-1 on timeout). `axis` is 'Y' or 'P'. namespace { struct LoadResult { double ms = -1; double cs_peak = 0, pwm_peak = 0, sg_min = 1e9; }; LoadResult moveAndSampleLoad(IMotorController& motor, const std::atomic& cancel, char axis, long target, std::chrono::milliseconds timeout) { LoadResult lr; motor.sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target)); std::this_thread::sleep_for(40ms); // let it leave standstill lr.ms = pollUntil( motor, cancel, [axis](const MotorTelemetry& t) { const AxisTelemetry& a = (axis == 'P') ? t.pitch : t.yaw; return a.standstill; }, [&](const MotorTelemetry& t) { const AxisTelemetry& a = (axis == 'P') ? t.pitch : t.yaw; lr.cs_peak = std::max(lr.cs_peak, (double)a.cs); lr.pwm_peak = std::max(lr.pwm_peak, (double)a.pwm); lr.sg_min = std::min(lr.sg_min, (double)a.sg); }, timeout); return lr; } } // namespace void TestRunner::testFriction(TestReport& r) { TestSection s; s.id = "friction"; s.title = "full-range traverse time + load"; const auto t_start = clock_t_::now(); const auto move_to = std::chrono::milliseconds(profile_.geti("settle_timeout_ms", 20000)); const int reps = std::max(1, profile_.geti("friction_reps", profile_.geti("reps", 5))); s.notes.push_back("load via ST sampling (firmware LP not implemented)"); auto runAxis = [&](char axis, double min_deg, double max_deg) { long lo = (axis == 'P') ? geo_.pitch.toCounts(min_deg) : geo_.yaw.toCounts(min_deg); long hi = (axis == 'P') ? geo_.pitch.toCounts(max_deg) : geo_.yaw.toCounts(max_deg); std::vector t_fwd, t_rev, l_fwd, l_rev; for (int i = 0; i < reps && !cancelled(); ++i) { setProgress("friction", i + 1, reps, axis == 'P' ? "pitch" : "yaw"); LoadResult f = moveAndSampleLoad(motor_, cancel_, axis, hi, move_to); LoadResult b = moveAndSampleLoad(motor_, cancel_, axis, lo, move_to); if (f.ms > 0) { t_fwd.push_back(f.ms); l_fwd.push_back(f.cs_peak); } if (b.ms > 0) { t_rev.push_back(b.ms); l_rev.push_back(b.cs_peak); } } const std::string a = (axis == 'P') ? "pitch" : "yaw"; if (!t_fwd.empty()) { add(s, a + "_traverse_ms_fwd", stats(t_fwd).mean, "ms"); add(s, a + "_traverse_ms_rev", stats(t_rev).mean, "ms"); add(s, a + "_load_peak_fwd", stats(l_fwd).max, "cs"); add(s, a + "_load_peak_rev", stats(l_rev).max, "cs"); double asym = std::fabs(stats(t_fwd).mean - stats(t_rev).mean); add(s, a + "_traverse_asym_ms", asym, "ms"); } }; runAxis('Y', geo_.yaw.min_deg, geo_.yaw.max_deg); { MotorTelemetry t = motor_.telemetry(); if (t.pitch_present) runAxis('P', geo_.pitch.min_deg, geo_.pitch.max_deg); } s.duration_ms = std::chrono::duration(clock_t_::now() - t_start).count(); rollup(s); r.sections.push_back(std::move(s)); } void TestRunner::testBacklash(TestReport& r) { TestSection s; s.id = "backlash"; s.title = "reversal dead-band"; const auto t_start = clock_t_::now(); const auto move_to = std::chrono::milliseconds(profile_.geti("settle_timeout_ms", 20000)); std::vector steps = profile_.getLongList("backlash_step_counts"); if (steps.empty()) steps = {500}; auto followErr = [&](char axis) -> long { MotorTelemetry t = motor_.telemetry(); const AxisTelemetry& a = (axis == 'P') ? t.pitch : t.yaw; return std::labs(a.xactual - a.xenc); }; auto stepMove = [&](char axis, long delta) { MotorTelemetry t = motor_.telemetry(); const AxisTelemetry& a = (axis == 'P') ? t.pitch : t.yaw; long target = a.xenc + delta; moveAndSampleLoad(motor_, cancel_, axis, target, move_to); }; auto runAxis = [&](char axis) { const std::string a = (axis == 'P') ? "pitch" : "yaw"; for (long step : steps) { if (cancelled()) return; setProgress("backlash", 1, (int)steps.size(), a.c_str()); // advance a few steps forward, then reverse and read the dead-band for (int k = 0; k < 3; ++k) stepMove(axis, +step); long mid_err = followErr(axis); stepMove(axis, -step); long reversal_err = followErr(axis); auto& m = add(s, a + "_deadband_s" + std::to_string(step), reversal_err - mid_err, "counts"); if (profile_.get("backlash_deadband_max_counts", 0) > 0) thresh(m, profile_.get("backlash_deadband_max_counts", 0)); } }; runAxis('Y'); { MotorTelemetry t = motor_.telemetry(); if (t.pitch_present) runAxis('P'); } s.duration_ms = std::chrono::duration(clock_t_::now() - t_start).count(); rollup(s); r.sections.push_back(std::move(s)); } void TestRunner::testBalance(TestReport& r) { TestSection s; s.id = "balance"; s.title = "pitch up/down current symmetry"; const auto t_start = clock_t_::now(); MotorTelemetry tp = motor_.telemetry(); if (!tp.pitch_present) { s.notes.push_back("no pitch axis present; skipped"); s.ran = false; r.sections.push_back(std::move(s)); return; } const auto move_to = std::chrono::milliseconds(profile_.geti("settle_timeout_ms", 20000)); const int N = std::max(2, profile_.geti("balance_steps", 10)); const double lo = geo_.pitch.min_deg, hi = geo_.pitch.max_deg; std::vector up(N, 0), down(N, 0); // upward for (int i = 0; i < N && !cancelled(); ++i) { setProgress("balance", i + 1, N, "up"); double deg = lo + (hi - lo) * (i + 1) / N; LoadResult lr = moveAndSampleLoad(motor_, cancel_, 'P', geo_.pitch.toCounts(deg), move_to); up[i] = lr.cs_peak; } // downward (reverse order) for (int i = N - 1; i >= 0 && !cancelled(); --i) { setProgress("balance", N - i, N, "down"); double deg = lo + (hi - lo) * i / N; LoadResult lr = moveAndSampleLoad(motor_, cancel_, 'P', geo_.pitch.toCounts(deg), move_to); down[i] = lr.cs_peak; } double max_asym = 0, sum_asym = 0; for (int i = 0; i < N; ++i) { double d = std::fabs(up[i] - down[i]); max_asym = std::max(max_asym, d); sum_asym += d; add(s, "step" + std::to_string(i) + "_up", up[i], "cs"); add(s, "step" + std::to_string(i) + "_down", down[i], "cs"); } add(s, "asym_mean", N ? sum_asym / N : 0, "cs"); thresh(add(s, "asym_max", max_asym, "cs"), profile_.get("balance_asym_max", 4)); s.duration_ms = std::chrono::duration(clock_t_::now() - t_start).count(); rollup(s); r.sections.push_back(std::move(s)); } // --------------------------------------------------------------------------- // IMU modules // --------------------------------------------------------------------------- void TestRunner::testImuConfig(TestReport& r) { TestSection s; s.id = "imu_config"; s.title = "IMU configuration"; if (!imu_) { s.notes.push_back("no IMU"); s.ran = false; r.sections.push_back(std::move(s)); return; } setProgress("imu_config", 1, 1, "reading"); imu_->refreshConfig(); auto cfg = imu_->config(); if (!cfg || !cfg->valid) { s.notes.push_back("device config unavailable"); thresh(add(s, "config_readable", 0, ""), 1); // lower_is_better default => fails (0<=1 true) s.metrics.back().lower_is_better = false; s.metrics.back().pass = false; s.ran = true; s.pass = false; r.sections.push_back(std::move(s)); return; } auto& c = *cfg; s.notes.push_back("device: " + c.product_code + " fw " + c.firmware); // Expected sample rate. double exp_hz = profile_.get("imu_expected_hz", 100); auto& mh = add(s, "sample_rate_hz", c.sample_rate_hz, "Hz", false); mh.has_threshold = true; mh.threshold = exp_hz * 0.95; mh.pass = std::fabs(c.sample_rate_hz - exp_hz) <= exp_hz * 0.05; // Output content present. auto& mo = add(s, "orientation_output", c.out_orientation ? 1 : 0, "", false); mo.has_threshold = true; mo.threshold = 1; mo.pass = c.out_orientation; // Active XKF profile matches expectation, if configured. std::string want = profile_.gets("imu_expected_profile"); if (!want.empty()) { auto& mp = add(s, "xkf_profile_match", c.scenario_label == want ? 1 : 0, "", false); mp.has_threshold = true; mp.threshold = 1; mp.pass = (c.scenario_label == want); if (!mp.pass) s.notes.push_back("XKF profile is '" + c.scenario_label + "', expected '" + want + "'"); } rollup(s); r.sections.push_back(std::move(s)); } namespace { // Collect IMU samples for `window`. Returns euler vectors + temp + dropped count. struct ImuStats { std::vector roll, pitch, yaw, temp; int dropped = 0, n = 0; double accel_norm_mean = 0; }; ImuStats sampleImu(IImuSource& imu, const std::atomic& cancel, std::chrono::milliseconds window) { ImuStats st; const auto deadline = clock_t_::now() + window; int last_counter = -1; double accel_sum = 0; int accel_n = 0; while (clock_t_::now() < deadline && !cancel.load()) { auto smp = imu.sample(); if (smp && smp->valid) { st.roll.push_back(smp->roll_deg); st.pitch.push_back(smp->pitch_deg); st.yaw.push_back(smp->yaw_deg); st.temp.push_back(smp->temp_c); double an = std::sqrt(smp->acc[0] * smp->acc[0] + smp->acc[1] * smp->acc[1] + smp->acc[2] * smp->acc[2]); accel_sum += an; ++accel_n; int cnt = smp->sample_counter; if (last_counter >= 0) { int gap = (cnt - last_counter) & 0xFFFF; if (gap > 1) st.dropped += gap - 1; } last_counter = cnt; ++st.n; } std::this_thread::sleep_for(5ms); } st.accel_norm_mean = accel_n ? accel_sum / accel_n : 0; return st; } } // namespace void TestRunner::testImuHealth(TestReport& r) { TestSection s; s.id = "imu_health"; s.title = "IMU health (static)"; if (!imu_) { s.notes.push_back("no IMU"); s.ran = false; r.sections.push_back(std::move(s)); return; } const auto window = std::chrono::milliseconds(profile_.geti("imu_sample_window_ms", 3000)); setProgress("imu_health", 1, 1, "sampling"); ImuStats st = sampleImu(*imu_, cancel_, window); if (st.n == 0) { s.notes.push_back("no IMU samples"); s.ran = true; s.pass = false; r.sections.push_back(std::move(s)); return; } double secs = window.count() / 1000.0; add(s, "sample_rate_hz", secs > 0 ? st.n / secs : 0, "Hz", false); thresh(add(s, "dropped_samples", st.dropped, "count"), profile_.get("imu_dropped_max", 0)); thresh(add(s, "yaw_noise", stats(st.yaw).sd, "deg"), profile_.get("imu_yaw_noise_max_deg", 0.5)); add(s, "pitch_noise", stats(st.pitch).sd, "deg"); add(s, "roll_noise", stats(st.roll).sd, "deg"); thresh(add(s, "accel_norm_err", std::fabs(st.accel_norm_mean - 9.81), "m/s2"), profile_.get("imu_accel_norm_err_max", 0.5)); auto& mt = add(s, "temperature", st.temp.empty() ? 0 : stats(st.temp).mean, "C"); if (profile_.get("imu_temp_max_c", 0) > 0) thresh(mt, profile_.get("imu_temp_max_c", 0)); rollup(s); r.sections.push_back(std::move(s)); } void TestRunner::testImuDrift(TestReport& r) { TestSection s; s.id = "imu_drift"; s.title = "IMU yaw drift (static)"; if (!imu_) { s.notes.push_back("no IMU"); s.ran = false; r.sections.push_back(std::move(s)); return; } const auto window = std::chrono::milliseconds(profile_.geti("imu_drift_window_ms", 60000)); setProgress("imu_drift", 1, 1, "drift window"); // Sample yaw vs time; least-squares slope (deg/min). std::vector ts, yaw; const auto t0 = clock_t_::now(); const auto deadline = t0 + window; double peak_min = 1e9, peak_max = -1e9; bool have_prev = false; double prev_deg = 0.0; while (clock_t_::now() < deadline && !cancelled()) { auto smp = imu_->sample(); if (smp && smp->valid) { double tmin = std::chrono::duration(clock_t_::now() - t0).count() / 60.0; // Yaw is reported raw, so it wraps at ±180. A rig parked on that // boundary would otherwise show a ~360° "drift" between consecutive // still samples; unwrapping makes the fit and the spread independent // of which heading the tower happens to face. double deg = smp->yaw_deg; if (have_prev) deg = unwrapNear(prev_deg, deg); prev_deg = deg; have_prev = true; ts.push_back(tmin); yaw.push_back(deg); peak_min = std::min(peak_min, deg); peak_max = std::max(peak_max, deg); } std::this_thread::sleep_for(20ms); } if (ts.size() < 3) { s.notes.push_back("insufficient IMU samples for drift fit"); s.ran = true; s.pass = false; r.sections.push_back(std::move(s)); return; } // slope = cov(t,yaw)/var(t) double mt = std::accumulate(ts.begin(), ts.end(), 0.0) / ts.size(); double my = std::accumulate(yaw.begin(), yaw.end(), 0.0) / yaw.size(); double cov = 0, var = 0; for (size_t i = 0; i < ts.size(); ++i) { cov += (ts[i] - mt) * (yaw[i] - my); var += (ts[i] - mt) * (ts[i] - mt); } double slope = var > 1e-12 ? cov / var : 0.0; // deg per minute thresh(add(s, "yaw_drift_deg_min", std::fabs(slope), "deg/min"), profile_.get("imu_yaw_drift_max_deg_min", 1.0)); add(s, "yaw_peak_excursion", peak_max - peak_min, "deg"); s.duration_ms = std::chrono::duration(clock_t_::now() - t0).count(); rollup(s); r.sections.push_back(std::move(s)); } // --------------------------------------------------------------------------- // Host module // --------------------------------------------------------------------------- void TestRunner::testHost(TestReport& r) { using namespace hostmetrics; if (selection_ & T_HOST_THERMAL) { TestSection s; s.id = "host_thermal"; s.title = "host CPU temperature"; ThermalInfo t = readThermal(); if (t.ok) { auto& m = add(s, "cpu_temp", t.max_temp_c, "C"); if (profile_.get("cpu_temp_max_c", 0) > 0) thresh(m, profile_.get("cpu_temp_max_c", 85)); add(s, "throttled", t.throttled ? 1 : 0, ""); s.notes.push_back("hottest zone: " + t.hottest_zone); } else s.notes.push_back("no thermal zones readable"); rollup(s); r.sections.push_back(std::move(s)); } if (selection_ & T_HOST_DISK) { TestSection s; s.id = "host_disk"; s.title = "image partition free space"; DiskInfo d = readDisk(disk_path_.empty() ? "." : disk_path_); if (d.ok) { auto& mf = add(s, "free_gb", d.free_gb, "GB", false); if (profile_.get("disk_free_min_gb", 0) > 0) thresh(mf, profile_.get("disk_free_min_gb", 5)); add(s, "used_pct", d.used_pct, "%"); } else s.notes.push_back("statvfs failed for " + d.path); rollup(s); r.sections.push_back(std::move(s)); } if (selection_ & T_HOST_MEMORY) { TestSection s; s.id = "host_memory"; s.title = "host memory"; MemInfo m = readMeminfo(); if (m.ok) { auto& ma = add(s, "ram_avail_mb", m.avail_mb, "MB", false); if (profile_.get("ram_avail_min_mb", 0) > 0) thresh(ma, profile_.get("ram_avail_min_mb", 256)); auto& ms = add(s, "swap_used_mb", m.swap_used_mb, "MB"); if (profile_.get("swap_used_max_mb", 0) > 0) thresh(ms, profile_.get("swap_used_max_mb", 0)); } else s.notes.push_back("/proc/meminfo unreadable"); rollup(s); r.sections.push_back(std::move(s)); } if (selection_ & T_HOST_LOAD) { TestSection s; s.id = "host_load"; s.title = "host load average"; LoadInfo l = readLoad(); if (l.ok) { add(s, "load1", l.load1, ""); add(s, "load5", l.load5, ""); add(s, "cpus", l.cpus, ""); if (l.cpus > 0) thresh(add(s, "load1_per_cpu", l.load1 / l.cpus, ""), profile_.get("load_per_cpu_max", 1.5)); } else s.notes.push_back("load average unavailable"); rollup(s); r.sections.push_back(std::move(s)); } } // --------------------------------------------------------------------------- // `test hardware` - is this rig the one hardware.ini says it is? // // These differ from every other leaf: they measure nothing and trend nothing. // Each is a precondition that either holds or does not, so each metric is a // 1/0 with a threshold of 1, and the detail lives in the section notes where an // operator reads it. namespace { // A pass/fail metric. Uses the same threshold machinery as the measured leaves // so rollup(), formatting and comparison all keep working unchanged. void verdict(TestSection& s, const std::string& name, const CheckResult& c) { if (!c.ran) { s.notes.push_back(name + ": not checked -- " + c.detail); return; } auto& m = add(s, name, c.pass ? 1 : 0, "", /*lower_is_better=*/false); thresh(m, 1); s.notes.push_back(name + ": " + c.detail); } // Pull one axis's echo out of a DUMP, or -1 when the axis or field is absent. long axisField(const DumpData& d, char code, long DumpAxis::*field) { for (const auto& ax : d.axes) if (ax.axis == code) return ax.*field; return -1; } long echoSteps(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::steps_per_rev); } long echoDev(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::enc_deviation); } long echoSwMode(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::sw_mode); } long echoHomingSpeed(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::homing_speed); } long echoCenterSpeed(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::center_speed); } long echoSpeed(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::speed); } long echoLimp(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::limp_home); } // The VMAX a declared deg/s becomes on this axis, or 0 when it cannot be formed. // Mirrors Application's fallback: the live calibration when it is trustworthy, // otherwise the one the declaration implies. long speedVmax(double deg_s, const AxisDrivetrainConfig& d, const AxisMap& map) { if (deg_s <= 0.0) return 0; const double live = std::fabs(map.counts_per_deg); const double cpd = live >= kMinPlausibleCountsPerDeg ? map.counts_per_deg : d.preHomeCountsPerDeg(); const auto p = planAxisSpeed(deg_s, cpd); return p.ok ? p.vmax : 0; } std::string readSysfs(const std::string& path) { if (path.empty()) return ""; std::ifstream f(path); if (!f) return ""; std::string line; std::getline(f, line); return line; } } // namespace void TestRunner::testHwAxes(TestReport& r) { TestSection s; s.id = "hw_axes"; s.title = "declared axes vs telemetry"; setProgress(s.id, 1, 1, "reading telemetry"); if (!hw_.declared || !hw_.declared->declared) { s.notes.push_back("no hardware.ini in force; nothing is declared to check against"); s.ran = false; r.sections.push_back(std::move(s)); return; } // One ST line is enough: yaw is implied by any well-formed line, pitch by // its own segment. MotorTelemetry t = motor_.telemetry(); FoundHardware f; // NEEDCFG counts as telemetry arriving: the link is fine and the axis is // talking, it just has not been told what it is driving. Treating it as "no // telemetry" would blame the serial port for a configuration gap. f.axes_known = t.yaw.state != AxisState::Unknown; if (f.axes_known) { f.axes.push_back("yaw"); if (t.pitch_present) f.axes.push_back("pitch"); } // Only the axis findings belong here; the sensors and cameras have their own // leaves, and a declaration they disagree with would otherwise fail twice. HardwareConfig axes_only; axes_only.declared = true; axes_only.source_path = hw_.declared->source_path; axes_only.axes = hw_.declared->axes; const PresenceReport pr = reconcilePresence(axes_only, f); CheckResult c; c.pass = !pr.anyMissing() && !pr.anyUndeclared(); if (c.pass) { c.detail = "telemetry carries exactly the declared axes (" + axes_only.axesLabel() + ")"; } else { for (const auto& line : formatPresence(pr)) c.detail += (c.detail.empty() ? "" : "; ") + line; } verdict(s, "axes_match", c); rollup(s); r.sections.push_back(std::move(s)); } void TestRunner::testHwMotor(TestReport& r) { TestSection s; s.id = "hw_motor"; s.title = "firmware adopted the declared drivetrain"; setProgress(s.id, 1, 1, "reading DUMP"); if (!hw_.drive) { s.notes.push_back("no drivetrain declared; firmware defaults are in force"); s.ran = false; r.sections.push_back(std::move(s)); return; } motor_.sendCommand("DUMP"); std::this_thread::sleep_for(250ms); const DumpData d = parseDump(motor_.lastDump()); if (r.fw.empty() && !d.build.empty()) r.fw = d.build; if (!d.valid) { CheckResult c; c.pass = false; c.detail = "no parseable DUMP came back, so what the firmware is running could not be " "confirmed"; verdict(s, "dump_readable", c); rollup(s); r.sections.push_back(std::move(s)); return; } auto check_axis = [&](const char* name, char code, const AxisDrivetrainConfig& cfg) { // First, because it explains the rest. An axis on limp-home values will // fail every comparison below, and that list of mismatched numbers reads // as "host and firmware disagree" - sending someone to re-send values // that were never sent at all. verdict(s, std::string(name) + "_not_limp", checkNotLimpHome(name, echoLimp(d, code))); DrivetrainExpectation want; if (cfg.run_current_ma > 0.0 && cfg.r_sense_ohm > 0.0) { const auto plan = planTmcCurrent(cfg.r_sense_ohm, cfg.run_current_ma, cfg.homing_current_ma); if (plan.ok) { want.declared = true; want.global_scaler = plan.global_scaler; want.run_irun = plan.run_irun; want.homing_irun = plan.homing_irun; } } DrivetrainEcho got; for (const auto& ax : d.axes) if (ax.axis == code) { got.global_scaler = ax.global_scaler; got.run_irun = ax.run_irun; got.homing_irun = ax.homing_irun; } verdict(s, std::string(name) + "_current", checkDrivetrainAdopted(name, want, got)); // Everything else the host declares and the firmware is told. Before the // config handshake these lived only in the firmware's own table, where a // stale or defaulted value was invisible from here. const AxisMap& map = (code == 'Y') ? hw_.geo_yaw : hw_.geo_pitch; const long want_normal = speedVmax(cfg.scan_speed_deg_s, cfg, map); const DeclaredValue vals[] = { {"steps_per_rev", cfg.steps_per_rev > 0, cfg.steps_per_rev, echoSteps(d, code)}, {"enc_deviation", cfg.enc_deviation > 0, cfg.enc_deviation, echoDev(d, code)}, {"sw_mode", cfg.wiring_declared, (long)cfg.sw_mode, echoSwMode(d, code)}, {"homing_speed", cfg.homing_speed_deg_s > 0, speedVmax(cfg.homing_speed_deg_s, cfg, map), echoHomingSpeed(d, code)}, {"center_speed", cfg.center_speed_deg_s > 0, speedVmax(cfg.center_speed_deg_s, cfg, map), echoCenterSpeed(d, code)}, {"normal_speed", want_normal > 0, want_normal, echoSpeed(d, code)}, }; verdict(s, std::string(name) + "_declared", checkDeclaredValues(name, vals, (int)(sizeof(vals) / sizeof(vals[0])))); // The wiring the image was built from, which no SET can change. if (hw_.declared) { const auto& p = hw_.declared->pins; const bool yaw = code == 'Y'; long cs = -1, en = -1; for (const auto& ax : d.axes) if (ax.axis == code) { cs = ax.cs_pin; en = ax.en_pin; } const DeclaredValue pins[] = { {"cs", (yaw ? p.yaw_cs : p.pitch_cs) >= 0, yaw ? p.yaw_cs : p.pitch_cs, cs}, {"en", (yaw ? p.yaw_en : p.pitch_en) >= 0, yaw ? p.yaw_en : p.pitch_en, en}, }; verdict(s, std::string(name) + "_pins", checkPinsBuilt(name, pins, 2)); } }; check_axis("yaw", 'Y', hw_.drive->yaw); if (!hw_.declared || hw_.declared->hasAxis("pitch")) check_axis("pitch", 'P', hw_.drive->pitch); // SPI is a choice only on the RP2040, so only there is it declared and echoed. if (hw_.declared && hw_.declared->pins.spi_sck >= 0) { const auto& p = hw_.declared->pins; const DeclaredValue spi[] = { {"spi_sck", true, p.spi_sck, d.spi_sck}, {"spi_mosi", p.spi_mosi >= 0, p.spi_mosi, d.spi_mosi}, {"spi_miso", p.spi_miso >= 0, p.spi_miso, d.spi_miso}, }; verdict(s, "spi_pins", checkPinsBuilt("spi", spi, 3)); } rollup(s); r.sections.push_back(std::move(s)); } void TestRunner::testHwImu(TestReport& r) { TestSection s; s.id = "hw_imu"; s.title = "declared IMU is present and streaming"; setProgress(s.id, 1, 1, "sampling"); const bool declared = hw_.declared && hw_.declared->declared && !hw_.declared->imu.empty() && hw_.declared->imu != "none"; if (!declared) { s.notes.push_back("no IMU declared"); s.ran = false; r.sections.push_back(std::move(s)); return; } CheckResult c; if (!imu_) { c.pass = false; c.detail = "declared as '" + hw_.declared->imu + "' but no IMU source was built -- check [IMU] device; the declaration itself is " "what enables it"; } else if (!imu_->connected()) { c.pass = false; c.detail = "source exists but is not connected -- the port opened without a 0xFA-framed " "stream arriving; check the baud and that it is the MTi's node"; } else if (!imu_->sample()) { c.pass = false; c.detail = "connected but no sample within the staleness window -- the stream stalled"; } else { c.detail = "'" + hw_.declared->imu + "' streaming"; } verdict(s, "imu_streaming", c); rollup(s); r.sections.push_back(std::move(s)); } void TestRunner::testHwEnv(TestReport& r) { TestSection s; s.id = "hw_env"; s.title = "declared ambient sensor, on a real I2C bus"; setProgress(s.id, 1, 2, "probing bus"); const bool declared = hw_.declared && hw_.declared->declared && !hw_.declared->env.empty() && hw_.declared->env != "none"; if (!declared) { s.notes.push_back("no ambient sensor declared"); s.ran = false; r.sections.push_back(std::move(s)); return; } // The adapter class first: an SMBus-only bus makes every later symptom look // like a dead sensor, so naming it first is what saves the debugging time. if (hw_.i2c_device.empty()) { CheckResult c; c.pass = false; c.detail = "declared as '" + hw_.declared->env + "' but [Env] i2c_device is empty"; verdict(s, "i2c_adapter", c); } else { I2cBus bus(hw_.i2c_device, 0x44); unsigned long funcs = 0; CheckResult c; if (!bus.open()) { c.pass = false; c.detail = "cannot open " + hw_.i2c_device + " -- wrong bus number, or the user is not in the i2c group"; } else if (!bus.functionality(funcs)) { c.pass = false; c.detail = "I2C_FUNCS ioctl failed on " + hw_.i2c_device; } else { c = checkI2cFunctionality(funcs); c.detail = hw_.i2c_device + ": " + c.detail; } verdict(s, "i2c_adapter", c); } setProgress(s.id, 2, 2, "reading sensor"); CheckResult m; if (!hw_.env) { m.pass = false; m.detail = "declared as '" + hw_.declared->env + "' but no sensor was built -- check [Env] i2c_device; the declaration itself is " "what enables it"; } else if (auto sample = hw_.env->sample()) { m.detail = "CRC-valid reading from " + hw_.env->name(); } else { m.pass = false; m.detail = "no valid reading within the staleness window -- every transfer is failing, or " "the CRC never checks out"; } verdict(s, "env_reading", m); rollup(s); r.sections.push_back(std::move(s)); } void TestRunner::testHwCamera(TestReport& r) { TestSection s; s.id = "hw_camera"; s.title = "cameras present, and the USB preconditions"; setProgress(s.id, 1, 2, "counting cameras"); // The USB preconditions are preconditions FOR A CAMERA. With none configured // and no source built, there is nothing they could be protecting, and // checking them would fail a dev box for a fault it cannot have. if (hw_.configured_cameras.empty() && !hw_.camera) { s.notes.push_back("no cameras configured; the USB preconditions have nothing to protect"); s.ran = false; r.sections.push_back(std::move(s)); return; } if (!hw_.configured_cameras.empty()) { CheckResult c; const int want = static_cast(hw_.configured_cameras.size()); const int got = hw_.camera ? hw_.camera->cameraCount() : -1; if (got < 0) { c.ran = false; c.detail = "no camera source in this run"; } else if (got < want) { c.pass = false; c.detail = std::to_string(want) + " configured, only " + std::to_string(got) + " opened -- one is not on the bus or is held by another process"; } else { c.detail = std::to_string(got) + " of " + std::to_string(want) + " opened"; } verdict(s, "cameras_found", c); } setProgress(s.id, 2, 2, "USB preconditions"); verdict(s, "usbfs_memory", checkUsbfsMemory(readSysfs(hw_.usbfs_path), hw_.usbfs_min_mb)); // LPM lives on a port node that differs per machine, so it is checked only // when the path is known. Silence here means "not located", never "fine". if (!hw_.lpm_path.empty()) { verdict(s, "usb3_lpm", checkUsb3Lpm(readSysfs(hw_.lpm_path))); } else { s.notes.push_back( "usb3_lpm: not checked -- no port node configured. Known-issues #19 has how to find " "and persist it; it re-enables on every reboot without a udev rule"); } rollup(s); r.sections.push_back(std::move(s)); } } // namespace fgc