761 lines
30 KiB
C++
761 lines
30 KiB
C++
#include "fgc/TestRunner.h"
|
|
|
|
#include "fgc/DiagParser.h"
|
|
#include "fgc/DumpParser.h"
|
|
#include "fgc/HostMetrics.h"
|
|
#include "fgc/IImuSource.h"
|
|
#include "fgc/IMotorController.h"
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <algorithm>
|
|
#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 == "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)
|
|
: motor_(motor), imu_(imu), geo_(geo), selection_(selection),
|
|
profile_(std::move(profile)), disk_path_(std::move(disk_path)) {}
|
|
|
|
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);
|
|
|
|
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;
|
|
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;
|
|
ts.push_back(tmin);
|
|
yaw.push_back(smp->yaw_deg);
|
|
peak_min = std::min(peak_min, (double)smp->yaw_deg);
|
|
peak_max = std::max(peak_max, (double)smp->yaw_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));
|
|
}
|
|
}
|
|
|
|
} // namespace fgc
|