1382 lines
58 KiB
C++
1382 lines
58 KiB
C++
#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 <algorithm>
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <cstdlib>
|
|
#include <numeric>
|
|
#include <thread>
|
|
|
|
#include <unistd.h> // 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::milliseconds>(
|
|
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<double>& v) {
|
|
Stats s;
|
|
s.n = static_cast<int>(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 == "camera") {
|
|
sub_ok = true;
|
|
if (test.empty()) m = T_CAM_ALL;
|
|
else if (test == "frames") m = T_CAM_FRAMES;
|
|
else if (test == "dark") m = T_CAM_DARK;
|
|
else if (test == "bright") m = T_CAM_BRIGHT;
|
|
else { err = "unknown camera 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<std::mutex> lk(result_mutex_);
|
|
report_.reset();
|
|
}
|
|
thread_ = std::thread([this] { run(); });
|
|
return true;
|
|
}
|
|
|
|
void TestRunner::cancel() { cancel_ = true; }
|
|
|
|
std::optional<TestReport> TestRunner::takeReport() {
|
|
std::lock_guard<std::mutex> lk(result_mutex_);
|
|
if (!report_) return std::nullopt;
|
|
auto out = std::move(report_);
|
|
report_.reset();
|
|
return out;
|
|
}
|
|
|
|
TestProgress TestRunner::progress() const {
|
|
std::lock_guard<std::mutex> lk(progress_mutex_);
|
|
return progress_;
|
|
}
|
|
|
|
void TestRunner::setProgress(const std::string& section, int step, int total,
|
|
const char* phase) {
|
|
std::lock_guard<std::mutex> 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<std::mutex> 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);
|
|
if (!cancelled() && (selection_ & T_CAM_ALL)) testCamera(r);
|
|
|
|
r.all_pass = true;
|
|
for (const auto& s : r.sections)
|
|
if (s.ran && !s.pass) r.all_pass = false;
|
|
|
|
{
|
|
std::lock_guard<std::mutex> 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 <typename Pred, typename Tick>
|
|
double pollUntil(IMotorController& motor, const std::atomic<bool>& 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<double, std::milli>(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<double> 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<double, std::milli>(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<double> 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<double>& peak, std::vector<double>& 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<double, std::milli>(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<bool>& 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<double> 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<double, std::milli>(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<long> 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<double, std::milli>(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<double> 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<double, std::milli>(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<double> roll, pitch, yaw, temp;
|
|
int dropped = 0, n = 0;
|
|
double accel_norm_mean = 0;
|
|
};
|
|
ImuStats sampleImu(IImuSource& imu, const std::atomic<bool>& 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<double> 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<double>(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<double, std::milli>(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<int>(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));
|
|
}
|
|
|
|
// `test camera` - frames, forced dark, and whether the sensor responds.
|
|
//
|
|
// The image test is deliberately not "is the picture good": fog, night and a
|
|
// lens cap all make a bad picture out of a healthy camera. Instead the camera's
|
|
// OWN settings drive it: minimum exposure and gain must take every pixel to
|
|
// black, and maximum gain with a long exposure must then produce a frame that is
|
|
// measurably different - which only a live sensor that took the settings can do,
|
|
// in daylight, at night or under a cap. Nothing is read into what the picture
|
|
// shows.
|
|
//
|
|
// A white end, forced by black level, was tried first and dropped: on tower 121's
|
|
// Alvium U-1620c it clips 100 % white, but tower 111's U-1240c has a black level
|
|
// range of only 0..255 of 4095 and cannot reach white by any setting at all.
|
|
void TestRunner::testCamera(TestReport& r) {
|
|
ICameraSource* cam = hw_.camera;
|
|
const int n = cam ? cam->cameraCount() : 0;
|
|
|
|
auto notRun = [&](const char* id, const char* title, const std::string& why) {
|
|
TestSection s; s.id = id; s.title = title;
|
|
s.notes.push_back(why);
|
|
s.ran = false;
|
|
r.sections.push_back(std::move(s));
|
|
};
|
|
const bool want_frames = selection_ & T_CAM_FRAMES;
|
|
const bool want_dark = selection_ & T_CAM_DARK;
|
|
const bool want_bright = selection_ & T_CAM_BRIGHT;
|
|
const char* kFramesTitle = "the camera delivers whole frames";
|
|
const char* kDarkTitle = "forced dark: every pixel follows to black";
|
|
const char* kBrightTitle = "forced bright: the sensor responds";
|
|
|
|
double lo = 0, hi = 0;
|
|
if (n == 0 || !cam->featureRange(0, "ExposureTime", lo, hi)) {
|
|
const std::string why = n == 0 ? "no camera in this run"
|
|
: "this camera source cannot be driven by the self-test";
|
|
if (want_frames) notRun("cam_frames", kFramesTitle, why);
|
|
if (want_dark) notRun("cam_dark", kDarkTitle, why);
|
|
if (want_bright) notRun("cam_bright", kBrightTitle, why);
|
|
return;
|
|
}
|
|
|
|
const int frames = static_cast<int>(profile_.get("camera_frames", 5));
|
|
const int discard = static_cast<int>(profile_.get("camera_settle_frames", 3));
|
|
const int warmup_ms = static_cast<int>(profile_.get("camera_warmup_ms", 10000));
|
|
const double min_pct = profile_.get("camera_clip_min_pct", 99.0);
|
|
const uint8_t black_max = static_cast<uint8_t>(profile_.get("camera_black_max", 20));
|
|
const double bright_us = profile_.get("camera_bright_exposure_us", 500000);
|
|
const double min_delta = profile_.get("camera_bright_min_delta", 2.0);
|
|
|
|
auto name = [&](int i) {
|
|
return i < static_cast<int>(hw_.configured_cameras.size()) ? hw_.configured_cameras[i]
|
|
: "cam" + std::to_string(i);
|
|
};
|
|
auto timeout = [&](int i) {
|
|
return i < static_cast<int>(hw_.frame_timeout_ms.size()) && hw_.frame_timeout_ms[i] > 0
|
|
? hw_.frame_timeout_ms[i] : 2000;
|
|
};
|
|
auto dropped = [&](int i) -> long long {
|
|
const auto info = cam->deviceInfo();
|
|
return i < static_cast<int>(info.size()) ? info[i].frames_dropped : 0;
|
|
};
|
|
// What the camera is exposing for right now. A frame cannot arrive faster
|
|
// than that: in a dark scene tower 111 auto-exposes for seconds, and a
|
|
// timeout that ignores it fails a healthy camera.
|
|
auto exposureUs = [&](int i) -> double {
|
|
const auto info = cam->deviceInfo();
|
|
return i < static_cast<int>(info.size()) ? info[i].exposure_us : 0.0;
|
|
};
|
|
auto num = [](double v) {
|
|
char b[32]; std::snprintf(b, sizeof(b), "%.6g", v); return std::string(b);
|
|
};
|
|
|
|
// The capture scheduler stopped the stream for the test; it needs to run.
|
|
cam->start();
|
|
|
|
TestSection sf, sd, sb;
|
|
sf.id = "cam_frames"; sf.title = kFramesTitle;
|
|
sd.id = "cam_dark"; sd.title = kDarkTitle;
|
|
sb.id = "cam_bright"; sb.title = kBrightTitle;
|
|
|
|
for (int i = 0; i < n && !cancelled(); ++i) {
|
|
const std::string cn = name(i);
|
|
|
|
// A freshly started stream is slow to deliver its first frame - 2 s on
|
|
// tower 111 at 1 fps and full resolution - so the first frame gets a
|
|
// warm-up allowance, and the rest are held to the profile's own timeout.
|
|
Frame first;
|
|
const bool warm = cam->grabFrame(i, first, warmup_ms);
|
|
|
|
if (want_frames) {
|
|
setProgress(sf.id, i + 1, n, "grabbing frames");
|
|
FrameRun run;
|
|
run.wanted = frames;
|
|
run.timeout_ms = timeout(i) + static_cast<int>(2 * exposureUs(i) / 1000);
|
|
const long long dropped0 = dropped(i);
|
|
const auto t0 = std::chrono::steady_clock::now();
|
|
for (int k = 0; k < frames && !cancelled() && warm; ++k) {
|
|
Frame f;
|
|
if (!cam->grabFrame(i, f, run.timeout_ms)) break;
|
|
if (run.got == 0) {
|
|
run.width = f.width; run.height = f.height; run.channels = f.channels;
|
|
} else if (f.width != run.width || f.height != run.height ||
|
|
f.channels != run.channels) {
|
|
run.consistent = false;
|
|
}
|
|
if (f.data.size() != static_cast<size_t>(f.width) * f.height * f.channels)
|
|
run.consistent = false;
|
|
++run.got;
|
|
}
|
|
const double secs = std::chrono::duration<double>(
|
|
std::chrono::steady_clock::now() - t0).count();
|
|
run.dropped = dropped(i) - dropped0;
|
|
CheckResult c = checkFramesDelivered(cn, run);
|
|
if (!warm) c.detail = cn + ": no first frame within " + std::to_string(warmup_ms) +
|
|
" ms of starting the stream";
|
|
verdict(sf, cn + "_frames", c);
|
|
if (run.got > 1 && secs > 0)
|
|
sf.notes.push_back(cn + ": " + std::to_string(run.got) + " frames in " +
|
|
std::to_string(static_cast<int>(secs * 1000)) + " ms");
|
|
}
|
|
|
|
if (!want_dark && !want_bright) continue;
|
|
|
|
// From here the camera is off its profile. It is put back below on every
|
|
// path out of these phases, and a failure to put it back is a failure of
|
|
// the test.
|
|
// Exposure and gain only. Both are writable while the camera streams;
|
|
// BlackLevel is not (on either Alvium here), and writing it meant
|
|
// restarting the stream mid-test - which on tower 111, slow to deliver a
|
|
// first frame and prone to stalling after a restart, left the test judging
|
|
// frames it could not vouch for. Black level stays at the profile's.
|
|
double emin = 0, emax = 0, gmin = 0, gmax = 0;
|
|
const bool ranges = cam->featureRange(i, "ExposureTime", emin, emax) &&
|
|
cam->featureRange(i, "Gain", gmin, gmax);
|
|
|
|
// Set, then judge a frame exposed well after the change: frames already
|
|
// in flight carry the old settings. Every grab is logged, so an odd
|
|
// verdict can be read back from the log rather than reconstructed.
|
|
auto forced = [&](const char* phase, double exposure, double gain,
|
|
PixelStats& out) -> std::string {
|
|
// A frame still being exposed under the OLD settings arrives after the
|
|
// change: allow for whichever exposure is longer, and count only
|
|
// frames that actually arrived before judging one. Measured on tower
|
|
// 111: with a 2+ s auto-exposure, the first frame after switching to
|
|
// the dark settings arrived 7 s later - and was nearly white.
|
|
const double before_us = exposureUs(i);
|
|
if (!ranges ||
|
|
!cam->setFeature(i, "ExposureAuto", "Off") ||
|
|
!cam->setFeature(i, "GainAuto", "Off") ||
|
|
!cam->setFeature(i, "ExposureTime", num(exposure)) ||
|
|
!cam->setFeature(i, "Gain", num(gain)))
|
|
return "the camera refused the forced settings (ExposureAuto, GainAuto, "
|
|
"ExposureTime, Gain)";
|
|
const int t = timeout(i) +
|
|
static_cast<int>(2 * std::max(before_us, exposure) / 1000);
|
|
const auto give_up = std::chrono::steady_clock::now() +
|
|
std::chrono::milliseconds(static_cast<long long>(t) * (discard + 2));
|
|
Frame f;
|
|
int got = 0;
|
|
for (int k = 0; got <= discard && !cancelled() &&
|
|
std::chrono::steady_clock::now() < give_up; ++k) {
|
|
Frame g;
|
|
if (cam->grabFrame(i, g, t)) {
|
|
LOG_DEBUG << "test camera: " << cn << ' ' << phase << " frame " << got
|
|
<< " ts " << g.timestamp_ms << " mean "
|
|
<< pixelStats(g.data, black_max, 250).mean;
|
|
f = std::move(g);
|
|
++got;
|
|
} else {
|
|
LOG_DEBUG << "test camera: " << cn << ' ' << phase << " wait " << k
|
|
<< " timed out after " << t << " ms";
|
|
}
|
|
}
|
|
if (got == 0) return "no frame arrived at the forced settings";
|
|
if (got <= discard)
|
|
return "only " + std::to_string(got) + " frame(s) arrived at the forced "
|
|
"settings - too few to be sure one was exposed after the change";
|
|
out = pixelStats(f.data, black_max, 250);
|
|
return "";
|
|
};
|
|
|
|
PixelStats dark;
|
|
std::string dark_err = "not taken";
|
|
if (!cancelled()) {
|
|
setProgress(sd.id, i + 1, n, "forcing dark");
|
|
dark_err = forced("dark", emin, gmin, dark);
|
|
}
|
|
if (want_dark) {
|
|
CheckResult c;
|
|
if (!dark_err.empty()) { c.pass = false; c.detail = cn + ": " + dark_err; }
|
|
else c = checkClipped(cn, false, dark, min_pct);
|
|
verdict(sd, cn + "_dark", c);
|
|
}
|
|
if (want_bright && !cancelled()) {
|
|
setProgress(sb.id, i + 1, n, "forcing bright");
|
|
PixelStats bright;
|
|
const std::string err = !dark_err.empty()
|
|
? "no dark frame to compare against (" + dark_err + ")"
|
|
: forced("bright", std::min(emax, bright_us), gmax, bright);
|
|
CheckResult c;
|
|
if (!err.empty()) { c.pass = false; c.detail = cn + ": " + err; }
|
|
else c = checkSensorResponds(cn, dark, bright, min_delta);
|
|
verdict(sb, cn + "_bright", c);
|
|
}
|
|
|
|
CheckResult back;
|
|
if (cam->restoreProfile(i)) {
|
|
back.detail = cn + ": back on its profile";
|
|
} else {
|
|
back.pass = false;
|
|
back.detail = cn + ": could NOT be put back on its profile - it is on the test's "
|
|
"forced settings; restart the program";
|
|
LOG_ERROR << "test camera: " << back.detail;
|
|
}
|
|
verdict(want_bright ? sb : sd, cn + "_restored", back);
|
|
}
|
|
|
|
cam->stop();
|
|
for (TestSection* s : {&sf, &sd, &sb}) {
|
|
if ((s == &sf && !want_frames) || (s == &sd && !want_dark) || (s == &sb && !want_bright))
|
|
continue;
|
|
rollup(*s);
|
|
r.sections.push_back(std::move(*s));
|
|
}
|
|
}
|
|
|
|
} // namespace fgc
|