2788 lines
132 KiB
C++
2788 lines
132 KiB
C++
#include "fgc/Application.h"
|
||
|
||
#include "fgc/PitchSetup.h"
|
||
#include "fgc/HeadingFit.h"
|
||
#include "fgc/CaptureScheduler.h"
|
||
#include "fgc/CommandParser.h"
|
||
#include "fgc/DiagParser.h"
|
||
#include "fgc/DumpParser.h"
|
||
#include "fgc/HelpText.h"
|
||
#include "fgc/Paths.h"
|
||
#include "fgc/CameraProfile.h"
|
||
#include "fgc/AxisSpeed.h"
|
||
#include "fgc/FirmwareConfig.h"
|
||
#include "fgc/HardwareChecks.h"
|
||
#include "fgc/HardwarePresence.h"
|
||
#include "fgc/TmcCurrent.h"
|
||
#include "fgc/ICameraSource.h"
|
||
#include "fgc/IControlChannel.h"
|
||
#include "fgc/IEnvSensor.h"
|
||
#include "fgc/IImuSource.h"
|
||
#include "fgc/IMotorController.h"
|
||
#include "fgc/ImagePipeline.h"
|
||
#include "fgc/Logger.h"
|
||
#include "fgc/MountedImuSource.h"
|
||
#include "fgc/MtiImuSource.h"
|
||
#include "fgc/ScanGrid.h"
|
||
#include "fgc/SerialMotorController.h"
|
||
#include "fgc/TestReport.h"
|
||
#include "fgc/TestRunner.h"
|
||
#include "fgc/mock/MockCameraSource.h"
|
||
#include "fgc/mock/MockEnvSensor.h"
|
||
#include "fgc/mock/MockImuSource.h"
|
||
#include "fgc/mock/MockMotorController.h"
|
||
#include "fgc/mock/NullControlChannel.h"
|
||
#include "fgc/sensors/Sht41EnvSensor.h"
|
||
#include "fgc/ui/HeadlessUi.h"
|
||
#include "fgc/ui/IUserInterface.h"
|
||
#include "fgc/ui/UiSnapshot.h"
|
||
|
||
#include <algorithm>
|
||
#include <atomic>
|
||
#include <cctype>
|
||
#include <chrono>
|
||
#include <cmath>
|
||
#include <csignal>
|
||
#include <cstdio>
|
||
#include <cstdlib>
|
||
#include <ctime>
|
||
#include <filesystem>
|
||
#include <fstream>
|
||
#include <memory>
|
||
#include <mutex>
|
||
#include <queue>
|
||
#include <sstream>
|
||
#include <string>
|
||
#include <thread>
|
||
|
||
#if FGC_WITH_MQTT
|
||
#include "fgc/MqttControlChannel.h"
|
||
#endif
|
||
#if FGC_WITH_VIMBA
|
||
#include "fgc/VimbaCameraSource.h"
|
||
#endif
|
||
#if FGC_WITH_TUI
|
||
#include "fgc/ui/TuiUi.h"
|
||
#endif
|
||
|
||
namespace fgc {
|
||
|
||
namespace {
|
||
|
||
// One encoder count in microsteps, rounded up. ENC_CONST is set so X_ENC advances
|
||
// in 12.8-microstep quanta, which is the finest position the gimbal can actually
|
||
// reproduce - the tolerance for any "is the count where we asked for it" check.
|
||
constexpr long kEncoderQuantumCounts = 13;
|
||
|
||
// Set by SIGINT/SIGTERM so the control loop exits cleanly and tears the camera down
|
||
// (StopContinuousImageAcquisition + Close). Without this, a Ctrl-C / kill / systemd
|
||
// stop skips teardown and leaves the camera acquiring - which wedges it so the NEXT
|
||
// run gets no frames. Only an atomic flag is set here (async-signal-safe).
|
||
std::atomic<bool> g_shutdown_requested{false};
|
||
void handleShutdownSignal(int) { g_shutdown_requested.store(true); }
|
||
|
||
long long nowEpochMs() {
|
||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch())
|
||
.count();
|
||
}
|
||
|
||
// "2026-09-11T14:32Z" - UTC, minute resolution. Stamped into the `;prev` comments
|
||
// saveMotorGeometry leaves behind, where the point is telling two calibrations
|
||
// apart later, not sub-second precision. UTC because towers are read remotely and
|
||
// a local-time stamp says nothing without also knowing the host's zone.
|
||
std::string utcStampNow() {
|
||
const std::time_t t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
|
||
std::tm tm{};
|
||
#if defined(_WIN32)
|
||
gmtime_s(&tm, &t);
|
||
#else
|
||
gmtime_r(&t, &tm);
|
||
#endif
|
||
// 64, not 32, purely to keep the compiler quiet. The real output is always 17
|
||
// characters, but the tm fields are plain ints, so GCC cannot rule out a
|
||
// nonsense clock and computes a worst case of 61 bytes - warning on every
|
||
// build. snprintf truncates rather than overflows either way, so this was
|
||
// never a safety problem; a recurring warning that everyone learns to ignore
|
||
// is the actual cost.
|
||
char b[64];
|
||
std::snprintf(b, sizeof(b), "%04d-%02d-%02dT%02d:%02dZ", tm.tm_year + 1900, tm.tm_mon + 1,
|
||
tm.tm_mday, tm.tm_hour, tm.tm_min);
|
||
return std::string(b);
|
||
}
|
||
|
||
// True if `dir` can be created and a probe file written there. A successful
|
||
// create_directories alone isn't enough (the dir may exist read-only), so we
|
||
// actually write+remove a probe file.
|
||
bool outputDirWritable(const std::string& dir) {
|
||
namespace fs = std::filesystem;
|
||
std::error_code ec;
|
||
fs::create_directories(dir, ec);
|
||
if (ec) return false;
|
||
const fs::path probe = fs::path(dir) / ".fgc_write_test";
|
||
std::ofstream f(probe);
|
||
if (!f.good()) return false;
|
||
f.close();
|
||
fs::remove(probe, ec);
|
||
return true;
|
||
}
|
||
|
||
// Human-readable list of the enabled wire-trace categories, for echoing back.
|
||
std::string traceNames(unsigned mask) {
|
||
if (mask == 0) return "none";
|
||
std::string out;
|
||
const std::pair<LogCat, const char*> all[] = {
|
||
{LogCat::Serial, "serial"}, {LogCat::Mqtt, "mqtt"},
|
||
{LogCat::Camera, "camera"}, {LogCat::Control, "control"},
|
||
};
|
||
for (const auto& [cat, name] : all) {
|
||
if (mask & static_cast<unsigned>(cat)) {
|
||
if (!out.empty()) out += ',';
|
||
out += name;
|
||
}
|
||
}
|
||
return out;
|
||
}
|
||
|
||
// Build the expanded-view ImuConfigView from the device's decoded config.
|
||
ImuConfigView formatImuConfig(const ImuDeviceConfig& c) {
|
||
ImuConfigView v;
|
||
v.present = true;
|
||
v.product_code = c.product_code.empty() ? "—" : c.product_code;
|
||
if (c.has_device_id) {
|
||
char b[16];
|
||
std::snprintf(b, sizeof(b), "0x%08X", c.device_id);
|
||
v.device_id = b;
|
||
} else {
|
||
v.device_id = "—";
|
||
}
|
||
v.firmware = c.firmware.empty() ? "—" : c.firmware;
|
||
|
||
if (c.has_output_mode) {
|
||
std::string m;
|
||
auto add = [&](bool on, const char* name) {
|
||
if (on) { if (!m.empty()) m += " · "; m += name; }
|
||
};
|
||
add(c.out_temperature, "Temp");
|
||
add(c.out_calibrated, "Calibrated");
|
||
add(c.out_orientation, "Orientation");
|
||
add(c.out_auxiliary, "Auxiliary");
|
||
add(c.out_status, "Status");
|
||
v.output_mode = m.empty() ? "—" : m;
|
||
} else {
|
||
v.output_mode = "—";
|
||
}
|
||
|
||
if (c.has_output_settings) {
|
||
v.output_settings = c.orientation_mode + " · " + c.timestamp_mode + " · " + c.data_format;
|
||
std::string ch;
|
||
if (c.acc_enabled) ch += "acc ";
|
||
if (c.gyr_enabled) ch += "gyr ";
|
||
if (c.mag_enabled) ch += "mag";
|
||
v.channels = ch.empty() ? "none" : ch;
|
||
} else {
|
||
v.output_settings = "—";
|
||
v.channels = "—";
|
||
}
|
||
|
||
if (c.has_period && c.sample_rate_hz > 0) {
|
||
char b[24];
|
||
std::snprintf(b, sizeof(b), "%.0f Hz", c.sample_rate_hz);
|
||
v.sample_rate = b;
|
||
} else {
|
||
v.sample_rate = "—";
|
||
}
|
||
|
||
// XKF profiles: list every available profile by name (no numeric IDs) and
|
||
// flag the active one (matched by profile type).
|
||
for (const auto& p : c.available_profiles) {
|
||
ImuProfileView pv;
|
||
pv.name = p.label.empty() ? "(unnamed)" : p.label;
|
||
pv.selected = c.has_scenario && p.type == c.scenario_type;
|
||
v.xkf_profiles.push_back(std::move(pv));
|
||
}
|
||
// If the device reported the active profile but not the full list, still show
|
||
// the one in use (by its resolved label, when known).
|
||
if (v.xkf_profiles.empty() && c.has_scenario && !c.scenario_label.empty())
|
||
v.xkf_profiles.push_back({c.scenario_label, true});
|
||
return v;
|
||
}
|
||
|
||
} // namespace
|
||
|
||
struct Application::Impl {
|
||
Impl(AppConfig c, RuntimeOptions o) : cfg(std::move(c)), opts(std::move(o)) {}
|
||
|
||
AppConfig cfg;
|
||
RuntimeOptions opts;
|
||
|
||
std::unique_ptr<IControlChannel> channel;
|
||
std::unique_ptr<IMotorController> motor;
|
||
std::unique_ptr<ICameraSource> camera;
|
||
// Camera modes, loaded once at startup and kept so a live switch needs no disk
|
||
// access. Keyed by camera name, then profile name; `active_profile` tracks
|
||
// which profile each camera is currently on.
|
||
std::map<std::string, std::map<std::string, CameraProfile>> camera_profiles;
|
||
std::map<std::string, std::string> active_profile;
|
||
std::unique_ptr<IImuSource> imu;
|
||
std::unique_ptr<IEnvSensor> env;
|
||
std::unique_ptr<ImagePipeline> pipeline;
|
||
std::unique_ptr<CaptureScheduler> scheduler;
|
||
std::unique_ptr<IUserInterface> ui;
|
||
std::unique_ptr<PitchSetupRoutine> calib;
|
||
std::unique_ptr<TestRunner> tester;
|
||
unsigned last_diag_seq = 0;
|
||
ScanGrid grid; // outlives scheduler (holds a reference to it)
|
||
std::string scan_load_error_; // non-empty if grid_file failed to load
|
||
|
||
// Persisted last-results for the TUI activity strip (survive in the log churn).
|
||
bool diag_running_ = false;
|
||
std::chrono::steady_clock::time_point diag_started_;
|
||
std::vector<std::string> last_diag_summary;
|
||
long long last_diag_ts = 0; // epoch ms
|
||
bool last_diag_pass = false;
|
||
DiagResultView last_diag_view; // per-axis worst tracking error
|
||
// Rising-edge latches for the live encoder-error WARN (avoid 100Hz spam).
|
||
bool yaw_track_over_ = false;
|
||
bool pitch_track_over_ = false;
|
||
CalibResultView last_calib_view;
|
||
std::vector<std::string> last_calib_summary;
|
||
long long last_calib_ts = 0; // epoch ms
|
||
// Motor-current registers computed once from hardware.ini, sent to the
|
||
// firmware before every home, and checked against the DUMP echo afterwards.
|
||
TmcCurrentPlan yaw_current_, pitch_current_;
|
||
bool calib_save_pending_ = false; // awaiting y/n to persist to config
|
||
// Which command produced the pending values, recorded into the `;prev`
|
||
// comment when they are saved so the file says what superseded what.
|
||
std::string calib_save_source_;
|
||
|
||
// Hardware self-test (`test` command) results, for the activity strip.
|
||
TestResultView last_test_view;
|
||
std::vector<std::string> last_test_summary;
|
||
long long last_test_ts = 0; // epoch ms
|
||
bool last_test_pass = false;
|
||
std::string last_test_report_path; // promoted by `test baseline`
|
||
|
||
// Procedure lock-out: while a long op runs, interfering commands are ignored
|
||
// and Esc cancels. Homing runs on the control thread (blocking), so it uses
|
||
// these flags directly; calibration/test run on their own threads.
|
||
std::atomic<bool> homing_active_{false};
|
||
std::atomic<bool> cancel_procedure_{false};
|
||
double test_drift_warn_ = 15.0; // % (from the running profile)
|
||
bool test_drift_gate_ = false;
|
||
mutable ImuConfigView imu_config_view_; // formatted MTi config (read once)
|
||
mutable bool imu_config_done_ = false;
|
||
// Latches the "no IMU sample, publishing encoder pitch" warning so an outage
|
||
// costs one line, not one per frame. Written from the camera delivery
|
||
// thread(s) via the orientation supplier, hence atomic.
|
||
std::atomic<bool> imu_pitch_warned_{false};
|
||
// The TUI heading wizard is aiming by hand; blocks a scan from starting.
|
||
bool heading_setup_active_ = false;
|
||
|
||
std::atomic<bool> running{true};
|
||
// --test: what run() returns, and where the one-shot test has got to.
|
||
int exit_code_ = 0;
|
||
bool resume_capture_after_test_ = false; // was capturing when a test began
|
||
bool oneshot_started_ = false;
|
||
std::chrono::steady_clock::time_point oneshot_since_{};
|
||
std::chrono::steady_clock::time_point oneshot_link_ok_{};
|
||
std::mutex cmd_mutex;
|
||
std::queue<std::string> cmd_queue;
|
||
|
||
std::chrono::steady_clock::time_point start_time;
|
||
std::mutex snapshot_mutex;
|
||
UiSnapshot latest_snapshot;
|
||
|
||
std::string statusTopicTower() const { return cfg.general.tower_name; }
|
||
|
||
std::unique_ptr<IControlChannel> makeChannel() {
|
||
bool want = opts.use_mqtt.value_or(cfg.features.enable_mqtt);
|
||
#if FGC_WITH_MQTT
|
||
if (want) {
|
||
return std::make_unique<MqttControlChannel>(cfg.network.brokerUri(),
|
||
cfg.general.tower_name,
|
||
cfg.network.mqtt_user,
|
||
cfg.network.mqtt_pw);
|
||
}
|
||
#else
|
||
if (want) LOG_WARN << "MQTT requested but binary built without MQTT; using null channel";
|
||
#endif
|
||
return std::make_unique<NullControlChannel>(cfg.general.tower_name);
|
||
}
|
||
|
||
std::unique_ptr<IMotorController> makeMotor() {
|
||
bool mock = opts.mock_serial.value_or(cfg.features.mock_serial);
|
||
if (mock) {
|
||
auto m = std::make_unique<MockMotorController>(cfg.features.mock_needcfg);
|
||
m->setBuiltPins(cfg.hardware.pins);
|
||
return m;
|
||
}
|
||
return std::make_unique<SerialMotorController>(cfg.serial.device, cfg.serial.baud);
|
||
}
|
||
|
||
std::unique_ptr<IImuSource> makeImu() {
|
||
if (!cfg.features.enable_imu) return nullptr; // sensors panel stays "pending"
|
||
|
||
std::unique_ptr<IImuSource> src;
|
||
if (cfg.features.mock_imu) {
|
||
src = std::make_unique<MockImuSource>();
|
||
} else if (cfg.imu.device.empty()) {
|
||
LOG_WARN << "IMU enabled but [IMU] device is empty; disabling IMU";
|
||
return nullptr;
|
||
} else {
|
||
src = std::make_unique<MtiImuSource>(cfg.imu.device, cfg.imu.baud);
|
||
}
|
||
|
||
// Wrapping here is what makes the mount universal: every consumer reads
|
||
// through IImuSource::sample(), so none of them can see the sensor frame.
|
||
// An unmounted rig skips the wrapper entirely rather than rotating by
|
||
// zero on every sample.
|
||
// The mock fabricates gimbal-frame readings directly, so only the real
|
||
// device carries an earth reference that has to be corrected.
|
||
const ImuReference ref = cfg.features.mock_imu ? ImuReference::Ned : kMtiReference;
|
||
if (ref == ImuReference::Ned && cfg.imu.mount.identity()) return src;
|
||
LOG_INFO << "IMU frames: reference " << referenceName(ref) << " -> NED; mount x "
|
||
<< cfg.imu.mount.x_deg << " y " << cfg.imu.mount.y_deg << " z "
|
||
<< cfg.imu.mount.z_deg << " (deg, Z-X-Y); readings are in the gimbal frame";
|
||
return std::make_unique<MountedImuSource>(std::move(src), ref, cfg.imu.mount);
|
||
}
|
||
|
||
std::unique_ptr<IEnvSensor> makeEnv() {
|
||
// --mock-env implies enable_env: the sensor is off by default, so without
|
||
// that the flag would silently do nothing.
|
||
const bool force_mock = opts.mock_env.value_or(false);
|
||
if (!cfg.features.enable_env && !force_mock)
|
||
return nullptr; // sensors panel stays "pending"
|
||
if (force_mock || cfg.features.mock_env)
|
||
return std::make_unique<MockEnvSensor>(std::chrono::milliseconds(cfg.env.period_ms));
|
||
if (cfg.env.i2c_device.empty()) {
|
||
LOG_WARN << "Env sensor enabled but [Env] i2c_device is empty; disabling";
|
||
return nullptr;
|
||
}
|
||
return std::make_unique<Sht41EnvSensor>(cfg.env.i2c_device,
|
||
static_cast<uint8_t>(cfg.env.i2c_addr),
|
||
std::chrono::milliseconds(cfg.env.period_ms));
|
||
}
|
||
|
||
// Does this run drive a real camera? Asked in two places - makeCamera() and
|
||
// the startup sequence, which must agree: profiles are meaningful only to
|
||
// VimbaCameraSource, so loading them for a mock run demands settings XMLs
|
||
// that a tower being brought up one subsystem at a time does not have yet.
|
||
//
|
||
// enable_camera substitutes the mock source rather than leaving `camera`
|
||
// null, because the capture path dereferences it in a dozen places. The
|
||
// result is a run with no camera hardware and no Vimba dependency, which is
|
||
// what the flag is for; it is not a silent no-op, and says so at startup.
|
||
bool realCameraWanted() const {
|
||
#if !FGC_WITH_VIMBA
|
||
return false;
|
||
#else
|
||
if (!cfg.features.enable_camera) return false;
|
||
return !opts.mock_camera.value_or(cfg.features.mock_camera);
|
||
#endif
|
||
}
|
||
|
||
std::unique_ptr<ICameraSource> makeCamera() {
|
||
bool mock = !realCameraWanted();
|
||
if (mock && !cfg.features.enable_camera) {
|
||
LOG_WARN << "camera disabled ([Features] enable_camera = false); using a mock source "
|
||
"- no images come from hardware in this run";
|
||
}
|
||
#if !FGC_WITH_VIMBA
|
||
if (!opts.mock_camera.value_or(cfg.features.mock_camera) && cfg.features.enable_camera) {
|
||
LOG_WARN << "Camera requested but binary built without Vimba X; using mock camera";
|
||
}
|
||
#endif
|
||
const auto active = cfg.camera.active();
|
||
if (mock) {
|
||
return std::make_unique<MockCameraSource>(
|
||
active.empty() ? 1 : static_cast<int>(active.size()));
|
||
}
|
||
#if FGC_WITH_VIMBA
|
||
std::vector<CameraSpec> specs;
|
||
std::vector<CameraProfile> profiles;
|
||
for (const auto* c : active) {
|
||
specs.push_back(*c);
|
||
profiles.push_back(camera_profiles.at(c->name).at(active_profile.at(c->name)));
|
||
}
|
||
return std::make_unique<VimbaCameraSource>(std::move(specs), std::move(profiles));
|
||
#else
|
||
return std::make_unique<MockCameraSource>(1);
|
||
#endif
|
||
}
|
||
|
||
// Load every profile of every camera up front. Two reasons: a profile nobody
|
||
// is using today still fails at startup rather than the first time someone
|
||
// switches to it, and a settings XML edited in the Viewer is detected and
|
||
// reported here rather than silently taking effect.
|
||
void loadProfiles() {
|
||
if (cfg.camera.cameras.empty())
|
||
throw std::runtime_error("no cameras configured (expected at least one "
|
||
"[Camera.<name>] block)");
|
||
|
||
for (const auto& c : cfg.camera.cameras) {
|
||
if (!c.enabled) continue;
|
||
std::map<std::string, fgc::ProfileLoadResult> results;
|
||
auto profiles = fgc::loadCameraProfiles(c.profile_dir, results);
|
||
|
||
if (profiles.find(c.profile) == profiles.end()) {
|
||
std::string known;
|
||
for (const auto& [name, p] : profiles) known += (known.empty() ? "" : ", ") + name;
|
||
throw std::runtime_error("Camera." + c.name + ".profile '" + c.profile +
|
||
"' has no " + c.profile + ".xml in " + c.profile_dir +
|
||
" (available: " + known + ")");
|
||
}
|
||
|
||
LOG_INFO << "camera '" << c.name << "': " << profiles.size() << " profile(s) from "
|
||
<< c.profile_dir << "; active '" << c.profile << "'";
|
||
for (const auto& [pname, r] : results) {
|
||
if (r.first_run) {
|
||
// Nothing changed - there was simply no record yet.
|
||
LOG_INFO << "camera '" << c.name << "' profile '" << pname
|
||
<< "': writing " << pname << ".txt (no previous record)";
|
||
continue;
|
||
}
|
||
// A changed XML is a warning, not an info line: the camera is about
|
||
// to behave differently from the last run and nobody edited this
|
||
// program.
|
||
LOG_WARN << "camera '" << c.name << "' profile '" << pname
|
||
<< "': settings XML changed, regenerating " << pname << ".txt";
|
||
for (const auto& line : r.changes) LOG_WARN << " " << line;
|
||
}
|
||
|
||
camera_profiles[c.name] = std::move(profiles);
|
||
active_profile[c.name] = c.profile;
|
||
}
|
||
}
|
||
|
||
// Switch one camera's profile by name. With no camera named, applies to the
|
||
// single configured camera. Returns false (and warns) if unknown.
|
||
bool applyProfile(const std::string& cam_name, const std::string& name) {
|
||
auto ci = camera_profiles.find(cam_name);
|
||
if (ci == camera_profiles.end()) {
|
||
std::string known;
|
||
for (const auto& [n, p] : camera_profiles) known += (known.empty() ? "" : ", ") + n;
|
||
LOG_WARN << "unknown camera '" << cam_name << "' (configured: " << known << ')';
|
||
return false;
|
||
}
|
||
auto it = ci->second.find(name);
|
||
if (it == ci->second.end()) {
|
||
std::string known;
|
||
for (const auto& [n, p] : ci->second) known += (known.empty() ? "" : ", ") + n;
|
||
LOG_WARN << "unknown profile '" << name << "' for camera '" << cam_name
|
||
<< "' (available: " << known << ')';
|
||
return false;
|
||
}
|
||
// The index is the camera's position among the ACTIVE cameras, which is the
|
||
// order the source opened them in and the order Frame::cam_id follows.
|
||
int index = -1, i = 0;
|
||
for (const auto* c : cfg.camera.active()) {
|
||
if (c->name == cam_name) { index = i; break; }
|
||
++i;
|
||
}
|
||
if (index < 0) {
|
||
LOG_WARN << "camera '" << cam_name << "' is configured but not enabled";
|
||
return false;
|
||
}
|
||
if (camera) camera->setProfile(index, it->second);
|
||
active_profile[cam_name] = name;
|
||
|
||
// The gimbal must stand still long enough for every camera, so recompute
|
||
// from all of them rather than from the one that just changed.
|
||
if (scheduler) {
|
||
int settle = 0, timeout = 0;
|
||
for (const auto* c : cfg.camera.active()) {
|
||
const auto t = c->timingFor(active_profile.count(c->name) ? active_profile[c->name]
|
||
: c->profile);
|
||
settle = std::max(settle, t.settle_wait_ms);
|
||
timeout = std::max(timeout, t.frame_timeout_ms);
|
||
}
|
||
scheduler->setSettleWait(settle, timeout);
|
||
}
|
||
LOG_INFO << "camera '" << cam_name << "' profile -> " << name;
|
||
return true;
|
||
}
|
||
|
||
// Comma-joined "label (type N)" for every XKF profile the device reports, so a
|
||
// bad argument can say what IS available instead of just refusing.
|
||
std::string imuProfileList() const {
|
||
auto cfg_opt = imu ? imu->config() : std::nullopt;
|
||
if (!cfg_opt || cfg_opt->available_profiles.empty()) return "(none reported)";
|
||
std::string out;
|
||
for (const auto& p : cfg_opt->available_profiles) {
|
||
if (!out.empty()) out += ", ";
|
||
out += (p.label.empty() ? std::string("?") : p.label) + " (type " +
|
||
std::to_string(static_cast<int>(p.type)) + ")";
|
||
if (cfg_opt->has_scenario && p.type == cfg_opt->scenario_type) out += " *";
|
||
}
|
||
return out;
|
||
}
|
||
|
||
// `imu profile [<type|label>]`. The MTi's XKF profile decides whether its
|
||
// heading uses the magnetometer. Note this does not affect the gimbal's own
|
||
// heading, which comes from the encoder via `gimbal heading setup`.
|
||
void imuProfile(const std::string& arg) {
|
||
if (arg.empty()) {
|
||
LOG_INFO << "XKF profiles (* = active): " << imuProfileList();
|
||
LOG_INFO << "select one with: imu profile <type|label>";
|
||
return;
|
||
}
|
||
|
||
int type = -1;
|
||
auto cfg_opt = imu->config();
|
||
|
||
// All-digits => a raw type number; anything else is a label, matched
|
||
// case-insensitively against what the device reported.
|
||
const bool numeric = arg.find_first_not_of("0123456789") == std::string::npos;
|
||
if (numeric) {
|
||
type = std::stoi(arg);
|
||
} else if (cfg_opt) {
|
||
auto lower = [](std::string v) {
|
||
std::transform(v.begin(), v.end(), v.begin(),
|
||
[](unsigned char ch) { return std::tolower(ch); });
|
||
return v;
|
||
};
|
||
const std::string want = lower(arg);
|
||
for (const auto& p : cfg_opt->available_profiles)
|
||
if (lower(p.label) == want) { type = p.type; break; }
|
||
}
|
||
|
||
if (type < 0) {
|
||
LOG_WARN << "unknown XKF profile '" << arg << "'; available: " << imuProfileList();
|
||
return;
|
||
}
|
||
|
||
LOG_INFO << "switching IMU to XKF profile type " << type
|
||
<< " (persists on the device; briefly pauses the IMU stream)";
|
||
const bool ok = imu->setFilterProfile(type);
|
||
// Either way the device state may have moved, so make the 'i' view re-read it
|
||
// rather than keep showing the profile we asked to leave.
|
||
imu_config_done_ = false;
|
||
if (ok)
|
||
LOG_INFO << "IMU XKF profile is now type " << type;
|
||
else
|
||
LOG_WARN << "IMU did not confirm XKF profile type " << type
|
||
<< " (older firmware may not report it; check the 'i' view)";
|
||
}
|
||
|
||
void imuStatus() {
|
||
if (!imu->connected()) {
|
||
LOG_WARN << "IMU: no recent sample (check [IMU] device and wiring)";
|
||
return;
|
||
}
|
||
auto sm = imu->sample();
|
||
if (!sm) { LOG_WARN << "IMU: no sample available"; return; }
|
||
LOG_INFO << "IMU: yaw " << formatDegrees(sm->yaw_deg) << " pitch "
|
||
<< formatDegrees(sm->pitch_deg) << " roll " << formatDegrees(sm->roll_deg);
|
||
char temp[32];
|
||
std::snprintf(temp, sizeof(temp), "%.1f", sm->temp_c);
|
||
LOG_INFO << " device temp " << temp << " C, sample #" << sm->sample_counter;
|
||
}
|
||
|
||
// Re-read before printing: the device is the authority, and a profile switched
|
||
// from anywhere else would otherwise be reported wrong.
|
||
void imuConfig() {
|
||
LOG_INFO << "re-reading IMU configuration (briefly pauses the stream)...";
|
||
imu->refreshConfig();
|
||
imu_config_done_ = false; // let the 'i' view pick the new values up too
|
||
auto c = imu->config();
|
||
if (!c) {
|
||
LOG_WARN << "IMU reported no configuration (older firmware may not answer the queries)";
|
||
return;
|
||
}
|
||
LOG_INFO << "IMU: " << (c->product_code.empty() ? "(unknown product)" : c->product_code)
|
||
<< " device id " << c->device_id << " firmware " << c->firmware;
|
||
if (c->has_period)
|
||
LOG_INFO << " sample rate " << c->sample_rate_hz << " Hz";
|
||
LOG_INFO << " XKF profile: "
|
||
<< (c->scenario_label.empty() ? std::string("(unnamed)") : c->scenario_label)
|
||
<< " (type " << static_cast<int>(c->scenario_type) << ')';
|
||
LOG_INFO << " available (* = active): " << imuProfileList();
|
||
const auto problems = imu->configProblems();
|
||
if (problems.empty()) {
|
||
LOG_INFO << " device matches the configuration the host asked for";
|
||
} else {
|
||
for (const auto& p : problems) LOG_ERROR << " " << p;
|
||
}
|
||
}
|
||
|
||
// `imu` command family, mirroring handleGimbal: the operator-facing surface of
|
||
// IImuSource. `imu norotation` + `imu zero` correct and re-zero the MTi's own
|
||
// heading; the gimbal's heading is independent of both.
|
||
void handleImu(const std::string& line) {
|
||
std::istringstream iss(line);
|
||
std::vector<std::string> tok;
|
||
std::string t;
|
||
while (iss >> t) tok.push_back(t); // tok[0] == "imu"
|
||
if (tok.size() < 2) {
|
||
LOG_WARN << "usage: imu <status|config|profile|zero|norotation>";
|
||
return;
|
||
}
|
||
if (!imu) {
|
||
LOG_WARN << "no IMU (declare [Hardware] imu, or use --mock-imu)";
|
||
return;
|
||
}
|
||
|
||
std::string sub = tok[1];
|
||
for (char& ch : sub) ch = static_cast<char>(std::tolower((unsigned char)ch));
|
||
|
||
if (sub == "status") {
|
||
imuStatus();
|
||
} else if (sub == "config") {
|
||
imuConfig();
|
||
} else if (sub == "profile") {
|
||
imuProfile(tok.size() > 2 ? tok[2] : "");
|
||
} else if (sub == "zero") {
|
||
// Yaw becomes relative to wherever the unit is pointing NOW, so say so:
|
||
// run mid-scan this silently re-references every later CamEvent heading.
|
||
LOG_INFO << "IMU heading reset: this pose is now yaw 0";
|
||
imu->headingReset();
|
||
} else if (sub == "norotation") {
|
||
int secs = 3; // matches CalibParams::norotation_s
|
||
if (tok.size() > 2) {
|
||
try { secs = std::stoi(tok[2]); } catch (...) {
|
||
LOG_WARN << "usage: imu norotation [<seconds>]";
|
||
return;
|
||
}
|
||
}
|
||
if (secs < 1) secs = 1;
|
||
// Non-blocking: the device runs the update on its own clock, so the
|
||
// operator - not this command - is what keeps the unit still.
|
||
LOG_INFO << "IMU gyro-bias update started: hold the unit still for " << secs << " s";
|
||
imu->noRotation(secs);
|
||
} else {
|
||
LOG_WARN << "unknown imu command '" << sub
|
||
<< "'; usage: imu <status|config|profile|zero|norotation>";
|
||
}
|
||
}
|
||
|
||
std::unique_ptr<IUserInterface> makeUi() {
|
||
bool want = opts.use_tui.value_or(cfg.ui.enable_tui);
|
||
#if FGC_WITH_TUI
|
||
if (want) return std::make_unique<TuiUi>();
|
||
#else
|
||
if (want) LOG_WARN << "TUI requested but binary built without TUI support; using headless console";
|
||
#endif
|
||
return std::make_unique<HeadlessUi>();
|
||
}
|
||
|
||
// Read live state (on the control thread) into a plain snapshot the UI can
|
||
// copy and render without touching the logic objects. The log pane is filled
|
||
// by the UI from its own ring buffer, so it is left empty here.
|
||
UiSnapshot buildSnapshot() const {
|
||
UiSnapshot s;
|
||
|
||
const bool mock_serial = opts.mock_serial.value_or(cfg.features.mock_serial);
|
||
s.header.tower = cfg.general.tower_name;
|
||
// Set by CMake from $GIT_REV, which deploy.sh passes in; "unknown" for a
|
||
// plain local build. This is the TUI header's build id.
|
||
s.header.build = FGC_GIT_REV;
|
||
s.header.uptime_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::steady_clock::now() - start_time).count();
|
||
s.header.live = !mock_serial;
|
||
|
||
// --- Gimbal ---
|
||
MotorTelemetry t = motor->telemetry();
|
||
s.gimbal.present = motor->connected();
|
||
s.gimbal.pitch_present = t.pitch_present;
|
||
// Yaw degrees are true compass bearings in [0,360); pitch is a signed
|
||
// elevation about level, so it must not wrap.
|
||
auto fillAxis = [](AxisView& v, const char* label, const AxisTelemetry& a,
|
||
const AxisMap& map, long target_counts, bool wrap) {
|
||
auto deg = [&](long counts) { return wrap ? map.headingDeg(counts) : map.toDeg(counts); };
|
||
v.label = label;
|
||
v.state = a.state;
|
||
v.deg = deg(a.xenc);
|
||
v.xactual = a.xactual;
|
||
v.xenc = a.xenc;
|
||
v.enc_err = a.xactual - a.xenc; // live tracking error (counts)
|
||
v.target_deg = deg(target_counts);
|
||
v.counts_per_deg = map.counts_per_deg;
|
||
v.sg = a.sg;
|
||
v.cs = a.cs;
|
||
v.pwm = a.pwm;
|
||
v.drv_status = a.drv_status;
|
||
v.moving = a.moving();
|
||
v.standstill = a.standstill;
|
||
v.stall = a.stall;
|
||
v.deviation = a.deviation;
|
||
v.overtemp = a.overtemp;
|
||
v.endstop_l = a.endstop_l;
|
||
v.endstop_r = a.endstop_r;
|
||
};
|
||
long yaw_tgt = scheduler ? scheduler->yawTargetCounts() : 0;
|
||
long pitch_tgt = scheduler ? scheduler->pitchTargetCounts() : 0;
|
||
fillAxis(s.gimbal.yaw, "YAW", t.yaw, cfg.geometry.yaw, yaw_tgt, true);
|
||
fillAxis(s.gimbal.pitch, "PITCH", t.pitch, cfg.geometry.pitch, pitch_tgt, false);
|
||
|
||
// Homing travel limits come from the last firmware dump (lim_neg/lim_pos);
|
||
// convert the endstop counts to degrees via the per-axis geometry.
|
||
DumpData dd = parseDump(motor->lastDump());
|
||
if (dd.valid) {
|
||
auto fillLimits = [](AxisView& v, const DumpAxis& a, const AxisMap& map, bool wrap) {
|
||
v.has_limits = true;
|
||
v.lim_neg = a.lim_neg;
|
||
v.lim_pos = a.lim_pos;
|
||
v.lim_neg_deg = wrap ? map.headingDeg(a.lim_neg) : map.toDeg(a.lim_neg);
|
||
v.lim_pos_deg = wrap ? map.headingDeg(a.lim_pos) : map.toDeg(a.lim_pos);
|
||
v.has_track = true;
|
||
v.hold_corrections = a.hold_corrections;
|
||
v.hold_peak_dev = a.hold_peak_dev;
|
||
v.hold_cumulative_dev = a.hold_cumulative_dev;
|
||
};
|
||
for (const auto& a : dd.axes) {
|
||
if (a.axis == 'Y') fillLimits(s.gimbal.yaw, a, cfg.geometry.yaw, true);
|
||
else if (a.axis == 'P') fillLimits(s.gimbal.pitch, a, cfg.geometry.pitch, false);
|
||
}
|
||
}
|
||
|
||
// --- Sensors: MTi (orientation) + SHT41 (ambient), each live if present ---
|
||
s.sensors = pendingSensorsView();
|
||
if (imu) {
|
||
auto fmt1 = [](float v) {
|
||
char b[24];
|
||
std::snprintf(b, sizeof(b), "%.1f", v);
|
||
return std::string(b);
|
||
};
|
||
auto setField = [&](SensorField& f, const std::string& v) {
|
||
f.value = v;
|
||
f.present = true;
|
||
};
|
||
// Device configuration (read once at startup): format and cache.
|
||
if (!imu_config_done_) {
|
||
if (auto c = imu->config()) {
|
||
imu_config_view_ = formatImuConfig(*c);
|
||
imu_config_done_ = true;
|
||
}
|
||
}
|
||
s.imu.config = imu_config_view_;
|
||
// From config.ini, not from the device, so it is available even when
|
||
// the MTi never answered the configuration queries.
|
||
s.imu.config.mount = formatImuMount(cfg.imu.mount);
|
||
// Also outside the cache above, and for a sharper reason: the handshake
|
||
// that produces these runs before the TUI installs its log sink, so the
|
||
// errors it logs are wiped by the alt-screen. This is the only place the
|
||
// operator can still see them.
|
||
s.imu.config.problems = imu->configProblems();
|
||
s.sensors.imu_config_alert = !s.imu.config.problems.empty();
|
||
if (auto m = imu->sample()) {
|
||
// Full reading for the expanded view.
|
||
s.imu.present = true;
|
||
s.imu.roll_deg = m->roll_deg;
|
||
s.imu.pitch_deg = m->pitch_deg;
|
||
s.imu.yaw_deg = m->yaw_deg;
|
||
for (int i = 0; i < 3; ++i) {
|
||
s.imu.acc[i] = m->acc[i];
|
||
s.imu.gyr[i] = m->gyr[i];
|
||
s.imu.mag[i] = m->mag[i];
|
||
}
|
||
s.imu.temp_c = m->temp_c;
|
||
s.imu.sample_counter = m->sample_counter;
|
||
// Compact MTi subsection: Roll/Pitch/Yaw only. The internal temp
|
||
// stays in s.imu (expanded view) so the main window's single
|
||
// temperature is unambiguously the SHT41's ambient one.
|
||
auto& g = s.sensors.imu;
|
||
g.present = true;
|
||
g.status = "live";
|
||
if (g.fields.size() >= 3) {
|
||
setField(g.fields[0], fmt1(m->roll_deg));
|
||
setField(g.fields[1], fmt1(m->pitch_deg));
|
||
setField(g.fields[2], fmt1(m->yaw_deg));
|
||
}
|
||
} else {
|
||
s.sensors.imu.status = "no fix";
|
||
}
|
||
}
|
||
|
||
if (env) {
|
||
auto fmt1 = [](float v) {
|
||
char b[24];
|
||
std::snprintf(b, sizeof(b), "%.1f", v);
|
||
return std::string(b);
|
||
};
|
||
auto setField = [&](SensorField& f, const std::string& v) {
|
||
f.value = v;
|
||
f.present = true;
|
||
};
|
||
// Identity and bus config are set whether or not a sample is available,
|
||
// so the expanded view can still say what the sensor is and where it
|
||
// should be when it goes quiet.
|
||
s.env.enabled = true;
|
||
s.env.mock = opts.mock_env.value_or(cfg.features.mock_env);
|
||
s.env.name = env->name();
|
||
s.env.i2c_device = cfg.env.i2c_device;
|
||
s.env.i2c_addr = cfg.env.i2c_addr;
|
||
s.env.period_ms = cfg.env.period_ms;
|
||
if (auto m = env->sample()) {
|
||
s.env.present = true;
|
||
s.env.temp_c = m->temp_c;
|
||
s.env.humidity_pct = m->humidity_pct;
|
||
s.env.timestamp_ms = m->timestamp_ms;
|
||
|
||
auto& g = s.sensors.env;
|
||
g.present = true;
|
||
g.status = "live";
|
||
if (g.fields.size() >= 2) {
|
||
setField(g.fields[0], fmt1(m->temp_c));
|
||
setField(g.fields[1], fmt1(m->humidity_pct));
|
||
}
|
||
} else {
|
||
s.sensors.env.status = "no fix";
|
||
}
|
||
}
|
||
|
||
// --- Camera / capture ---
|
||
s.capture.present = true;
|
||
s.capture.active = scheduler && scheduler->captureActive();
|
||
// Manual = no scan running. Drives what the key bar advertises; the
|
||
// enforcement is in requireManual(), not in the UI.
|
||
s.capture.manual = manualMode();
|
||
s.capture.snapshot_armed = scheduler && scheduler->snapshotPending();
|
||
s.capture.image_rate = scheduler ? scheduler->imageRate() : 0.0;
|
||
s.capture.camera_count = camera ? camera->cameraCount() : 0;
|
||
s.capture.labels.clear();
|
||
for (const auto* c : cfg.camera.active()) s.capture.labels.push_back(c->label);
|
||
// Per-camera configuration, for the expanded camera view.
|
||
{
|
||
auto& cc = s.capture.config;
|
||
cc.mock = opts.mock_camera.value_or(cfg.features.mock_camera);
|
||
cc.output_dir = cfg.paths.output_dir;
|
||
cc.cameras.clear();
|
||
for (const auto& c : cfg.camera.cameras) {
|
||
CameraView v;
|
||
v.name = c.name;
|
||
v.label = c.label;
|
||
v.id = c.id;
|
||
v.driver = c.driver;
|
||
v.enabled = c.enabled;
|
||
v.profile_dir = c.profile_dir;
|
||
v.rotation_deg = c.rotation_deg;
|
||
v.jxl_distance = c.jxl_distance;
|
||
v.jxl_effort = c.jxl_effort;
|
||
auto ap = active_profile.find(c.name);
|
||
v.profile = ap != active_profile.end() ? ap->second : c.profile;
|
||
v.settle_wait_ms = c.timingFor(v.profile).settle_wait_ms;
|
||
if (auto ci = camera_profiles.find(c.name); ci != camera_profiles.end()) {
|
||
for (const auto& [pname, prof] : ci->second)
|
||
v.profiles_available.push_back(pname);
|
||
if (auto pi = ci->second.find(v.profile); pi != ci->second.end())
|
||
v.profile_xml = pi->second.xml_path;
|
||
}
|
||
cc.cameras.push_back(std::move(v));
|
||
}
|
||
}
|
||
// Live per-camera identity/telemetry/stats.
|
||
s.capture.devices.clear();
|
||
if (camera) {
|
||
for (const auto& d : camera->deviceInfo()) {
|
||
CameraDeviceView v;
|
||
v.id = d.id; v.model = d.model; v.serial = d.serial; v.streaming = d.streaming;
|
||
v.width = d.width; v.height = d.height; v.payload_bytes = d.payload_bytes;
|
||
v.exposure_us = d.exposure_us; v.gain_db = d.gain_db;
|
||
v.temperature_c = d.temperature_c; v.actual_fps = d.actual_fps;
|
||
v.frames_delivered = d.frames_delivered; v.frames_dropped = d.frames_dropped;
|
||
v.restarts = d.restarts;
|
||
s.capture.devices.push_back(std::move(v));
|
||
}
|
||
}
|
||
s.capture.images_saved = pipeline ? pipeline->imagesSaved() : 0;
|
||
|
||
// Defined scan grid + live cursor (read on the control thread, same as the
|
||
// scheduler that mutates it, so no locking is needed).
|
||
s.capture.scan_from_file = !cfg.scan.grid_file.empty();
|
||
s.capture.scan_pitch = t.pitch_present;
|
||
s.capture.scan_error = scan_load_error_;
|
||
s.capture.scan_grid.clear();
|
||
const auto& pts = grid.points();
|
||
s.capture.scan_grid.reserve(pts.size());
|
||
for (std::size_t i = 0; i < pts.size(); ++i)
|
||
s.capture.scan_grid.push_back({pts[i].yaw_deg, pts[i].pitch_deg, i == grid.index()});
|
||
if (pipeline) {
|
||
if (auto ev = pipeline->lastEvent()) {
|
||
s.capture.has_last = true;
|
||
s.capture.last_label = ev->camera;
|
||
s.capture.last_heading_deg = ev->heading_decideg / 10.0;
|
||
s.capture.last_pitch_deg = ev->pitch_decideg / 10.0;
|
||
s.capture.last_ts_ms = ev->timestamp_ms;
|
||
}
|
||
}
|
||
|
||
// --- Connectivity ---
|
||
s.conn.mqtt_enabled = opts.use_mqtt.value_or(cfg.features.enable_mqtt);
|
||
s.conn.mqtt_connected = channel && channel->connected();
|
||
s.conn.broker = cfg.network.brokerUri();
|
||
s.conn.tower = cfg.general.tower_name;
|
||
s.conn.control_code = scheduler ? scheduler->controlCode() : 0;
|
||
s.conn.target_heading = scheduler ? scheduler->targetHeading() : "0";
|
||
s.conn.last_status_code = s.conn.control_code; // echoed back as status
|
||
if (channel) {
|
||
s.conn.recent = channel->recentMessages();
|
||
s.conn.stats = channel->channelStats();
|
||
}
|
||
|
||
// --- Diagnostics (last firmware DUMP) ---
|
||
s.dump.text = motor->lastDump();
|
||
s.dump.has = !s.dump.text.empty();
|
||
|
||
// --- Activity strip + last calibration ---
|
||
s.calib = last_calib_view;
|
||
s.calib.pitch_present = s.gimbal.pitch_present || s.calib.pitch_present;
|
||
// The map in force right now, whether or not anything was fitted today.
|
||
s.calib.live_yaw = {cfg.geometry.yaw.counts_per_deg, cfg.geometry.yaw.deg_at_center};
|
||
s.calib.live_pitch = {cfg.geometry.pitch.counts_per_deg, cfg.geometry.pitch.deg_at_center};
|
||
s.diag = last_diag_view;
|
||
s.test = last_test_view;
|
||
fillActivity(s);
|
||
return s;
|
||
}
|
||
|
||
// The currently-running special op (priority: calib > diag > homing > capture)
|
||
// plus the most recent persisted result, for the activity strip.
|
||
void fillActivity(UiSnapshot& s) const {
|
||
ActivityView& a = s.activity;
|
||
|
||
if (tester && tester->running()) {
|
||
TestProgress p = tester->progress();
|
||
a.active = true;
|
||
a.title = "TESTING";
|
||
a.status = p.section.empty()
|
||
? "running hardware self-test…"
|
||
: p.section + " " + std::to_string(p.step) + "/" +
|
||
std::to_string(p.total) + " — " + p.phase;
|
||
} else if (calib && calib->running()) {
|
||
CalibProgress p = calib->progress();
|
||
a.active = true;
|
||
a.title = "CALIBRATING";
|
||
std::string axis = (p.axis == 'P') ? "PITCH" : (p.axis == 'Y') ? "YAW" : "";
|
||
a.status = axis.empty() ? p.phase
|
||
: axis + " step " + std::to_string(p.step) + "/" +
|
||
std::to_string(p.total) + " — " + p.phase;
|
||
} else if (diag_running_) {
|
||
a.active = true;
|
||
a.title = "DIAGNOSTICS";
|
||
a.status = "running motor self-test…";
|
||
} else if (s.gimbal.yaw.state == AxisState::Homing ||
|
||
(s.gimbal.pitch_present && s.gimbal.pitch.state == AxisState::Homing)) {
|
||
a.active = true;
|
||
a.title = "HOMING";
|
||
a.status = "finding endstops…";
|
||
} else if (scheduler && scheduler->captureActive()) {
|
||
a.active = true;
|
||
a.title = "CAPTURE";
|
||
const char* phase = scheduler->moving() ? "moving" : "dwell";
|
||
if (scheduler->controlCode() == 1) {
|
||
a.status = "directed → heading " + scheduler->targetHeading() + "° · " +
|
||
(scheduler->moving() ? "moving" : "holding");
|
||
} else if (!grid.empty()) {
|
||
const ScanPoint& p = grid.current();
|
||
std::string coord = "yaw " + formatDegrees(p.yaw_deg);
|
||
if (s.capture.scan_pitch) coord += " / pitch " + formatDegrees(p.pitch_deg);
|
||
a.status = "waypoint " + std::to_string(grid.index() + 1) + "/" +
|
||
std::to_string(grid.size()) + " · " + coord + " · " + phase;
|
||
} else {
|
||
a.status = "no scan grid — auto-sweep disabled";
|
||
}
|
||
}
|
||
|
||
// A running procedure is cancelable with Esc.
|
||
a.cancelable = procedureBusy();
|
||
|
||
// Persisted result: the most recent of last test / calibration / diagnostics.
|
||
const long long now = nowEpochMs();
|
||
if (last_test_ts >= last_calib_ts && last_test_ts >= last_diag_ts && last_test_ts) {
|
||
a.has_result = true;
|
||
a.result_title = std::string("Test ") + (last_test_pass ? "PASS" : "FAIL") +
|
||
" · " + formatTimeAgo(now, last_test_ts);
|
||
a.result = last_test_summary;
|
||
a.result_color = last_test_pass ? UiColor::Green : UiColor::Red;
|
||
} else if (last_calib_ts || last_diag_ts) {
|
||
const bool calib_newer = last_calib_ts >= last_diag_ts;
|
||
a.has_result = true;
|
||
if (calib_newer) {
|
||
a.result_title = "Calibration · " + formatTimeAgo(now, last_calib_ts);
|
||
a.result = last_calib_summary;
|
||
a.result_color = last_calib_view.has &&
|
||
last_calib_view.yaw.ok ? UiColor::Green : UiColor::Yellow;
|
||
} else {
|
||
a.result_title = std::string("Diagnostics ") +
|
||
(last_diag_pass ? "PASS" : "FAIL") + " · " +
|
||
formatTimeAgo(now, last_diag_ts);
|
||
a.result = last_diag_summary;
|
||
a.result_color = last_diag_pass ? UiColor::Green : UiColor::Red;
|
||
}
|
||
}
|
||
|
||
// Pending yes/no: persist the just-applied calibration as the new default?
|
||
if (calib_save_pending_)
|
||
// Name the source: a rough derivation and a landmark fit both arrive
|
||
// here, and they do not deserve the same confidence.
|
||
a.prompt = "Save the new [Motor] calibration (" +
|
||
(calib_save_source_.empty() ? std::string("calibration")
|
||
: calib_save_source_) +
|
||
") to config as the default? (y / n)";
|
||
}
|
||
|
||
void publishSnapshot() {
|
||
UiSnapshot s = buildSnapshot();
|
||
std::lock_guard<std::mutex> lock(snapshot_mutex);
|
||
latest_snapshot = std::move(s);
|
||
}
|
||
|
||
// Warn (once per excursion) when an axis's live encoder error |xactual-xenc|
|
||
// crosses the configured limit, so positional slip is visible in the log
|
||
// without watching the gimbal view. Hysteresis re-arms at 75% of the limit.
|
||
void checkTrackingError() {
|
||
const long limit = cfg.enc_error_warn_counts;
|
||
if (limit <= 0) return; // disabled
|
||
MotorTelemetry t = motor->telemetry();
|
||
auto check = [&](const char* axis, const AxisTelemetry& a, bool present, bool& over) {
|
||
if (!present) return;
|
||
const long err = std::labs(a.xactual - a.xenc);
|
||
if (!over && err > limit) {
|
||
over = true;
|
||
LOG_WARN << "encoder tracking error on " << axis << ": " << (a.xactual - a.xenc)
|
||
<< " counts (limit " << limit << ")";
|
||
} else if (over && err < limit * 3 / 4) {
|
||
over = false;
|
||
}
|
||
};
|
||
check("yaw", t.yaw, true, yaw_track_over_);
|
||
check("pitch", t.pitch, t.pitch_present, pitch_track_over_);
|
||
}
|
||
|
||
void runInitSequence() {
|
||
using namespace std::chrono_literals;
|
||
LOG_INFO << "Running gimbal init sequence (enable + home)";
|
||
// Mark homing active so interfering commands are ignored and Esc cancels.
|
||
// This blocks the control thread, so commands are drained+discarded here.
|
||
homing_active_ = true;
|
||
cancel_procedure_ = false;
|
||
struct ClearHoming {
|
||
std::atomic<bool>& flag;
|
||
~ClearHoming() { flag = false; }
|
||
} clear_homing{homing_active_};
|
||
|
||
motor->sendCommand("ENABLE Y");
|
||
// Only address axes this tower actually has. A yaw-only rig answers
|
||
// "ERR axis not present" to every pitch command, which arrives in the log
|
||
// as a row of warnings at every startup - noise that trains an operator to
|
||
// ignore exactly the channel that reports real faults.
|
||
if (cfg.hardware.hasAxis("pitch")) motor->sendCommand("ENABLE P");
|
||
// Tell the firmware where to anchor the homed midpoint and how much
|
||
// clearance to keep from each endstop, BEFORE homing: finishHoming() uses
|
||
// the centre count to re-origin, so a late send would apply to the next
|
||
// home, not this one.
|
||
sendAnchorConfig();
|
||
|
||
// Confirm the current landed BEFORE driving into anything. This is the
|
||
// only check standing between a stale firmware and an endstop that cannot
|
||
// survive full torque.
|
||
// Only when a reduction is actually depended on. An axis running at one
|
||
// current for everything has nothing to lose if the command went astray,
|
||
// and the check costs a DUMP round trip whose output floods the log.
|
||
if (cfg.drivetrain.yaw.reducesForHoming() || cfg.drivetrain.pitch.reducesForHoming()) {
|
||
if (auto dd = freshDump(std::chrono::seconds(3))) {
|
||
if (!verifyCurrentAdoption(*dd)) return;
|
||
} else {
|
||
LOG_WARN << "no firmware dump before homing; cannot confirm the motor "
|
||
"current was adopted";
|
||
}
|
||
}
|
||
|
||
motor->sendCommand("HOME"); // homes all axes; firmware runs it non-blocking
|
||
|
||
auto allReady = [](const MotorTelemetry& t) {
|
||
return t.yaw.ready() && (!t.pitch_present || t.pitch.ready());
|
||
};
|
||
|
||
// An axis re-homed from READY is still READY for a moment, so first wait
|
||
// briefly for homing to actually begin (an axis leaves READY) before
|
||
// waiting for it to finish - otherwise we'd see the residual READY and
|
||
// return early having waited for nothing.
|
||
const auto t0 = std::chrono::steady_clock::now();
|
||
while (running && !cancel_procedure_ && std::chrono::steady_clock::now() < t0 + 3s) {
|
||
std::this_thread::sleep_for(100ms);
|
||
drainAndDiscardCommands(); // ignore moves/etc queued during homing
|
||
publishSnapshot(); // keep the TUI's gimbal panel live during homing
|
||
if (!allReady(motor->telemetry())) break; // homing started
|
||
}
|
||
|
||
// Wait for completion (both axes READY again) or the homing timeout.
|
||
bool homed = false;
|
||
// How long a full two-end search may take is a property of the drivetrain,
|
||
// not of this program: a high gear ratio homing at a deliberately gentle
|
||
// speed takes minutes, and a fixed 65 s ceiling reported those towers as
|
||
// failed while the firmware was still searching perfectly happily.
|
||
const auto deadline =
|
||
std::chrono::steady_clock::now() + std::chrono::seconds(cfg.homing_timeout_s);
|
||
while (running && !cancel_procedure_ && std::chrono::steady_clock::now() < deadline) {
|
||
std::this_thread::sleep_for(250ms);
|
||
drainAndDiscardCommands();
|
||
publishSnapshot(); // keep the TUI's gimbal panel live during homing
|
||
MotorTelemetry t = motor->telemetry();
|
||
if (t.yaw.state == AxisState::Error ||
|
||
(t.pitch_present && t.pitch.state == AxisState::Error)) {
|
||
LOG_WARN << "Homing reported ERROR state; check the gimbal";
|
||
break;
|
||
}
|
||
if (allReady(t)) {
|
||
LOG_INFO << "Gimbal homed (both axes READY)";
|
||
homed = true;
|
||
break;
|
||
}
|
||
}
|
||
if (cancel_procedure_) {
|
||
LOG_WARN << "Homing cancelled";
|
||
motor->sendCommand("STOP ALL");
|
||
return;
|
||
}
|
||
// Re-anchor the yaw zero to an endstop now that the homed limits are known,
|
||
// and take the chance to check the degree scale against the travel we just
|
||
// measured. Derivation first: applyHomedTravel() converts the soft limits
|
||
// through the map, so it wants the corrected scale.
|
||
if (homed) {
|
||
using namespace std::chrono_literals;
|
||
if (auto dd = freshDump(3s)) {
|
||
deriveCountsPerDeg(*dd);
|
||
applyHomedTravel(&*dd);
|
||
} else {
|
||
LOG_WARN << "no firmware dump after homing; degree limits and the derived "
|
||
"counts_per_deg both skipped";
|
||
}
|
||
}
|
||
// Production move speeds for subsequent MOVE commands, converted from the
|
||
// declared deg/s. Sent here rather than earlier because deriveCountsPerDeg()
|
||
// above has just refreshed the scale this conversion depends on.
|
||
sendProductionSpeeds();
|
||
LOG_INFO << "Gimbal init finished";
|
||
}
|
||
|
||
// Translate each axis's declared deg/s into the VMAX this drivetrain needs,
|
||
// and tell the firmware.
|
||
//
|
||
// The speed is declared as a physical quantity because a register value is
|
||
// not portable: the same VMAX is 21.8 deg/s on the Delta fleet and 6.0 deg/s
|
||
// on the IOTA, and both were carrying the same literal. What is logged is
|
||
// what the axis will actually do, not what was asked for - the two differ by
|
||
// the integer quantisation, and by more than that when a request is clamped.
|
||
void sendProductionSpeeds() {
|
||
auto one = [&](const char* name, char code, const AxisMap& map,
|
||
const AxisDrivetrainConfig& d) {
|
||
if (!cfg.hardware.hasAxis(name)) return;
|
||
const auto p = planAxisSpeed(d.scan_speed_deg_s, map.counts_per_deg);
|
||
if (!p.ok) {
|
||
// Leave the firmware on its compile-time default rather than
|
||
// commanding a speed derived from a scale we do not trust.
|
||
LOG_WARN << name << " speed: not set -- " << p.error
|
||
<< "; the firmware keeps its built-in default";
|
||
return;
|
||
}
|
||
motor->sendCommand(std::string("SPEED ") + code + " " + std::to_string(p.vmax));
|
||
if (p.clamped)
|
||
LOG_WARN << name << " speed: " << d.scan_speed_deg_s
|
||
<< " deg/s exceeds what VMAX can express on this drivetrain; clamped to "
|
||
<< p.actual_deg_s << " deg/s";
|
||
LOG_INFO << name << " speed: " << p.actual_deg_s << " deg/s (VMAX " << p.vmax
|
||
<< ") -- " << map.counts_per_deg << " counts/deg";
|
||
};
|
||
one("yaw", 'Y', cfg.geometry.yaw, cfg.drivetrain.yaw);
|
||
one("pitch", 'P', cfg.geometry.pitch, cfg.drivetrain.pitch);
|
||
}
|
||
|
||
// Build what the host can answer for one axis, for the config handshake.
|
||
//
|
||
// Speeds are converted here rather than declared as VMAX, for the reason in
|
||
// AxisSpeed.h. Homing and centring happen BEFORE the first home, so they
|
||
// cannot use the counts_per_deg that homing derives - they use the scale the
|
||
// declaration implies (steps_per_rev / travel_deg), which on the IOTA agrees
|
||
// with the measured one to 0.25%.
|
||
AxisConfigSource axisConfigSource(const char* axis, const AxisMap& map,
|
||
const AxisDrivetrainConfig& d, const TmcCurrentPlan& cur,
|
||
long soft_margin) {
|
||
AxisConfigSource s;
|
||
s.have_current = cur.ok;
|
||
s.global_scaler = cur.global_scaler;
|
||
s.run_irun = cur.run_irun;
|
||
s.homing_irun = cur.homing_irun;
|
||
|
||
s.center_count = map.center_count;
|
||
s.soft_margin = soft_margin;
|
||
|
||
s.have_travel = d.steps_per_rev > 0;
|
||
s.steps_per_rev = d.steps_per_rev;
|
||
s.have_deviation = d.enc_deviation > 0;
|
||
s.enc_deviation = d.enc_deviation;
|
||
|
||
s.have_wiring = d.wiring_declared;
|
||
s.sw_mode = d.sw_mode;
|
||
s.has_encoder = d.has_encoder;
|
||
|
||
// The scale to convert through. The firmware asks for all three speeds at
|
||
// BOOT, before anything has been homed, so the live calibration is often
|
||
// still AxisMap's uncalibrated default - fall back to the one the
|
||
// declaration implies. Homing supersedes it: sendProductionSpeeds() re-sends
|
||
// the production speed against the measured scale once it exists.
|
||
const double live = std::fabs(map.counts_per_deg);
|
||
const double cpd = live >= kMinPlausibleCountsPerDeg ? map.counts_per_deg
|
||
: d.preHomeCountsPerDeg();
|
||
auto vmax = [&](double deg_s) -> long {
|
||
if (deg_s <= 0.0) return 0; // undeclared: firmware default stands
|
||
const auto p = planAxisSpeed(deg_s, cpd);
|
||
if (!p.ok) {
|
||
LOG_WARN << axis << " " << deg_s << " deg/s could not be converted: " << p.error;
|
||
return 0;
|
||
}
|
||
return p.vmax;
|
||
};
|
||
s.normal_vmax = vmax(d.scan_speed_deg_s);
|
||
s.homing_vmax = vmax(d.homing_speed_deg_s);
|
||
s.center_vmax = vmax(d.center_speed_deg_s);
|
||
return s;
|
||
}
|
||
|
||
// Answer any NEED lines the firmware has emitted.
|
||
//
|
||
// The firmware stays in NEEDCFG and moves nothing until its required set is
|
||
// complete, so an unanswered key is a startup failure rather than a warning:
|
||
// it is named here, loudly, because the fix is in hardware.ini and nothing
|
||
// else will make the gimbal move.
|
||
void answerFirmwareNeeds() {
|
||
for (const auto& line : motor->takeNeedLines()) {
|
||
const auto need = parseNeedLine(line);
|
||
if (!need) {
|
||
LOG_WARN << "unparseable NEED line from the firmware: " << line;
|
||
continue;
|
||
}
|
||
answerNeed(*need, /*asked=*/true);
|
||
}
|
||
}
|
||
|
||
// Every key the host can answer, by its canonical name. Used to re-send the
|
||
// whole declared set on connect - see resendDeclaredConfig().
|
||
static NeedRequest fullRequest(char axis) {
|
||
NeedRequest r;
|
||
r.axis = axis;
|
||
r.keys = {"steps_per_rev", "global_scaler", "run_irun", "homing_irun",
|
||
"center_count", "soft_margin", "enc_deviation", "normal_speed",
|
||
"homing_speed", "center_speed", "sw_mode", "has_encoder"};
|
||
return r;
|
||
}
|
||
|
||
// Push the declared drivetrain at the firmware whether or not it asked.
|
||
//
|
||
// The firmware asks exactly once, at boot, and only while it is in NEEDCFG.
|
||
// Without this, editing hardware.ini and restarting the HOST changes nothing
|
||
// - the board is already configured, so it says nothing, and the host has
|
||
// nothing to reply to. The tower keeps running the old values silently.
|
||
// Re-tuning would need a re-flash, which is seven seconds on a bench and a
|
||
// trip up a mast in the field.
|
||
//
|
||
// CENTER and MARGIN are sent again as SET here, having just gone out as
|
||
// their own commands. That is idempotent and costs two lines; keeping the
|
||
// set uniform is worth more than saving them.
|
||
//
|
||
// It also makes the handshake robust to a NEED line lost on the wire: an
|
||
// axis still in NEEDCFG is completed by this re-send rather than sitting
|
||
// there refusing to move.
|
||
void resendDeclaredConfig() {
|
||
for (const char* name : {"yaw", "pitch"}) {
|
||
if (!cfg.hardware.hasAxis(name)) continue;
|
||
answerNeed(fullRequest(name[0] == 'y' ? 'Y' : 'P'), /*asked=*/false);
|
||
}
|
||
}
|
||
|
||
// `asked` distinguishes the two callers. When the firmware ASKED, a key the
|
||
// host cannot supply is a startup failure - the axis stays in NEEDCFG and
|
||
// will not move, so it is an error. On a proactive re-send it means only
|
||
// "hardware.ini does not declare this one", and the firmware simply keeps
|
||
// what it already had; saying ERROR there would cry wolf on every start.
|
||
void answerNeed(const NeedRequest& need, bool asked) {
|
||
const bool is_yaw = need.axis == 'Y';
|
||
|
||
// An axis the tower says it does not have. That is not a missing
|
||
// declaration - it is a firmware built for more axes than this rig,
|
||
// and answering would configure a driver that is not there. Same
|
||
// class of fault as presence reconciliation catches, so it is named
|
||
// that way rather than as a hole in hardware.ini.
|
||
const char* axis_name = is_yaw ? "yaw" : "pitch";
|
||
if (cfg.hardware.declared && !cfg.hardware.hasAxis(axis_name)) {
|
||
LOG_ERROR << "firmware asked for axis " << need.axis << " (" << axis_name
|
||
<< "), which this tower does not declare (axes "
|
||
<< cfg.hardware.axesLabel()
|
||
<< ") -- the firmware was built for more axes than the rig has; "
|
||
"rebuild it for this board rather than declaring an axis that is "
|
||
"not fitted";
|
||
return;
|
||
}
|
||
const auto src = is_yaw
|
||
? axisConfigSource("yaw", cfg.geometry.yaw, cfg.drivetrain.yaw,
|
||
yaw_current_, cfg.yaw_soft_margin_counts)
|
||
: axisConfigSource("pitch", cfg.geometry.pitch,
|
||
cfg.drivetrain.pitch, pitch_current_,
|
||
cfg.pitch_soft_margin_counts);
|
||
|
||
const auto plan = planFirmwareConfig(need, src);
|
||
for (const auto& cmd : plan.sets) motor->sendCommand(cmd);
|
||
|
||
if (!plan.unanswered.empty() && asked) {
|
||
std::string keys;
|
||
for (const auto& k : plan.unanswered) keys += (keys.empty() ? "" : ", ") + k;
|
||
LOG_ERROR << "firmware asked axis " << need.axis << " for " << keys
|
||
<< " and hardware.ini does not declare "
|
||
<< (plan.unanswered.size() == 1 ? "it" : "them")
|
||
<< "; the axis stays in NEEDCFG and will not move";
|
||
}
|
||
if (!plan.unknown.empty() && asked) {
|
||
std::string keys;
|
||
for (const auto& k : plan.unknown) keys += (keys.empty() ? "" : ", ") + k;
|
||
LOG_ERROR << "firmware asked axis " << need.axis << " for " << keys
|
||
<< ", which this host does not know -- the firmware is newer than the "
|
||
"host; upgrade the host rather than editing hardware.ini";
|
||
}
|
||
if (asked && plan.complete())
|
||
LOG_INFO << "firmware config: answered " << plan.sets.size() << " keys for axis "
|
||
<< need.axis;
|
||
if (!asked && !plan.sets.empty())
|
||
LOG_INFO << "drivetrain re-sent to axis " << need.axis << ": " << plan.sets.size()
|
||
<< " values from hardware.ini";
|
||
}
|
||
|
||
// Convert this tower's declared milliamps into TMC register fields, once.
|
||
// The host owns this because hardware.ini is where the sense resistor lives;
|
||
// the firmware is told registers and keeps working compile-time defaults.
|
||
void planMotorCurrents() {
|
||
auto plan = [&](const char* name, const AxisDrivetrainConfig& d, TmcCurrentPlan& out) {
|
||
if (!d.declared || d.run_current_ma <= 0.0) return;
|
||
out = planTmcCurrent(d.r_sense_ohm, d.run_current_ma, d.homing_current_ma);
|
||
for (const auto& w : out.warnings) LOG_WARN << name << " motor current: " << w;
|
||
if (!out.ok) {
|
||
LOG_ERROR << name << " motor current could not be computed; the firmware "
|
||
"will keep its built-in defaults";
|
||
return;
|
||
}
|
||
// Log what the registers actually deliver, not what was asked for.
|
||
LOG_INFO << name << " motor current: run " << std::lround(out.run_ma)
|
||
<< " mA (GLOBALSCALER " << out.global_scaler << ", IRUN " << out.run_irun
|
||
<< "), homing " << std::lround(out.homing_ma) << " mA (IRUN "
|
||
<< out.homing_irun << ") -- R_sense " << d.r_sense_ohm
|
||
<< " ohm from hardware.ini [Motor." << name << ']';
|
||
};
|
||
plan("yaw", cfg.drivetrain.yaw, yaw_current_);
|
||
plan("pitch", cfg.drivetrain.pitch, pitch_current_);
|
||
}
|
||
|
||
// Did the firmware actually adopt the current we sent? GLOBALSCALER and
|
||
// IHOLD_IRUN are write-only on the TMC5160, so the DUMP echo is the only
|
||
// channel - and sendCommand() is fire-and-forget, so an 'ERR unknown command'
|
||
// from an older firmware arrives as an uncorrelated warning we cannot pin to
|
||
// this command. Returns false only when a reduction was REQUIRED and is
|
||
// demonstrably absent.
|
||
bool verifyCurrentAdoption(const DumpData& dd) {
|
||
bool safe = true;
|
||
auto check = [&](const char* name, char axis, const AxisDrivetrainConfig& d,
|
||
const TmcCurrentPlan& want) {
|
||
if (!want.ok) return;
|
||
const DumpAxis* ax = nullptr;
|
||
for (const auto& a : dd.axes)
|
||
if (a.axis == axis) ax = &a;
|
||
if (!ax) return;
|
||
|
||
const bool absent = (ax->run_irun < 0);
|
||
const bool wrong = !absent && (ax->global_scaler != want.global_scaler ||
|
||
ax->run_irun != want.run_irun ||
|
||
ax->homing_irun != want.homing_irun);
|
||
if (!absent && !wrong) return;
|
||
|
||
std::ostringstream detail;
|
||
if (absent)
|
||
detail << "the firmware does not report a motor current at all, so it "
|
||
"predates the CURRENT command";
|
||
else
|
||
detail << "the firmware reports GLOBALSCALER " << ax->global_scaler << ", IRUN "
|
||
<< ax->run_irun << '/' << ax->homing_irun << " but was sent "
|
||
<< want.global_scaler << ", IRUN " << want.run_irun << '/'
|
||
<< want.homing_irun;
|
||
|
||
if (d.reducesForHoming()) {
|
||
// The reduction is load-bearing: this tower's endstop yields to
|
||
// full torque. Homing anyway is how the stop gets destroyed.
|
||
LOG_ERROR << name << " motor current was NOT adopted -- " << detail.str()
|
||
<< ". Refusing to home: this axis declares a reduced homing "
|
||
"current because its endstop cannot survive full torque. "
|
||
"Flash the firmware, or remove homing_current_ma from "
|
||
"hardware.ini to accept the risk deliberately.";
|
||
safe = false;
|
||
} else {
|
||
LOG_WARN << name << " motor current was not adopted -- " << detail.str()
|
||
<< ". Continuing: this axis asks for no reduction, so homing is "
|
||
"unaffected.";
|
||
}
|
||
};
|
||
check("yaw", 'Y', cfg.drivetrain.yaw, yaw_current_);
|
||
check("pitch", 'P', cfg.drivetrain.pitch, pitch_current_);
|
||
return safe;
|
||
}
|
||
|
||
// Tell the firmware the anchor point and endstop clearance for each axis.
|
||
// The centre count is what finishHoming() re-origins onto, so all counts stay
|
||
// positive and reproducible across power cycles; the margin is what
|
||
// clampTarget() keeps clear of each hard limit.
|
||
void sendAnchorConfig() {
|
||
motor->sendCommand("CENTER Y " + std::to_string(cfg.geometry.yaw.center_count));
|
||
motor->sendCommand("MARGIN Y " + std::to_string(cfg.yaw_soft_margin_counts));
|
||
if (cfg.hardware.hasAxis("pitch")) {
|
||
motor->sendCommand("CENTER P " + std::to_string(cfg.geometry.pitch.center_count));
|
||
motor->sendCommand("MARGIN P " + std::to_string(cfg.pitch_soft_margin_counts));
|
||
}
|
||
// Motor current belongs here too: it governs homing torque, so it has to
|
||
// land before the search, not after it.
|
||
auto sendCurrent = [&](char axis, const TmcCurrentPlan& p) {
|
||
if (!p.ok) return;
|
||
motor->sendCommand(std::string("CURRENT ") + axis + ' ' +
|
||
std::to_string(p.global_scaler) + ' ' +
|
||
std::to_string(p.run_irun) + ' ' +
|
||
std::to_string(p.homing_irun));
|
||
};
|
||
sendCurrent('Y', yaw_current_);
|
||
sendCurrent('P', pitch_current_);
|
||
|
||
// And the rest of the declared drivetrain. CENTER/MARGIN/CURRENT above
|
||
// have always been re-sent on every start; the keys the config handshake
|
||
// added were not, so half the tuning took effect on a restart and half
|
||
// silently did not. This makes all of it behave the same way.
|
||
resendDeclaredConfig();
|
||
}
|
||
|
||
// Derive each axis's reachable degree window from the freshly homed soft
|
||
// limits. These are deliberately NOT configurable: the firmware's limits are
|
||
// the only place they can be correct, and a stale configured range would
|
||
// either advertise travel that does not exist or hide travel that does.
|
||
//
|
||
// Also verifies that the firmware actually adopted the anchor and margin we
|
||
// sent. With no EEPROM the limits can only come from a home in this power
|
||
// cycle, so a midpoint that is not center_count means the CENTER command did
|
||
// not take effect - firmware predating it answers "ERR unknown command" and
|
||
// homes to its own compiled default, which would leave every published
|
||
// bearing silently wrong.
|
||
// Request a DUMP and wait briefly for a parseable one. Several callers need
|
||
// the firmware's own view of itself - the homed limits, and whether it adopted
|
||
// what we sent - so the round trip lives in one place.
|
||
// What this run actually has, for reconciliation against hardware.ini.
|
||
// Called once at startup, after every subsystem has been constructed and
|
||
// started, because "found" can only mean "came up", not "was configured".
|
||
FoundHardware observeHardware(std::chrono::milliseconds wait) {
|
||
using namespace std::chrono_literals;
|
||
FoundHardware f;
|
||
|
||
// Axes come from telemetry, which takes a moment to arrive. Yaw reaching
|
||
// any real state is the signal the link is alive; until then a
|
||
// default-constructed snapshot would report every axis missing.
|
||
MotorTelemetry t = motor->telemetry();
|
||
const auto deadline = std::chrono::steady_clock::now() + wait;
|
||
while (running && t.yaw.state == AxisState::Unknown &&
|
||
std::chrono::steady_clock::now() < deadline) {
|
||
std::this_thread::sleep_for(100ms);
|
||
t = motor->telemetry();
|
||
}
|
||
f.axes_known = t.yaw.state != AxisState::Unknown;
|
||
if (f.axes_known) {
|
||
f.axes.push_back("yaw"); // any well-formed ST line carries Y:
|
||
if (t.pitch_present) f.axes.push_back("pitch");
|
||
}
|
||
|
||
// A null source is the "not fitted / not enabled" case; makeImu() and
|
||
// makeEnv() already log why.
|
||
f.imu = imu != nullptr;
|
||
f.env = env != nullptr;
|
||
|
||
for (const auto* c : cfg.camera.active()) f.cameras.push_back(c->name);
|
||
f.cameras_opened = camera ? camera->cameraCount() : -1;
|
||
return f;
|
||
}
|
||
|
||
// Compare the rig against its declaration and act on the difference.
|
||
// Returns false when the run must not continue.
|
||
bool reconcileHardware() {
|
||
using namespace std::chrono_literals;
|
||
if (!cfg.hardware.declared) return true; // nothing declared, nothing to check
|
||
|
||
const PresenceReport report = reconcilePresence(cfg.hardware, observeHardware(3s));
|
||
for (const auto& line : formatPresence(report)) LOG_WARN << "hardware: " << line;
|
||
|
||
const PresenceVerdict v =
|
||
presenceVerdict(report, cfg.features.strict_presence, opts.degraded);
|
||
switch (v.action) {
|
||
case PresenceAction::Proceed:
|
||
LOG_INFO << "hardware: declared assembly confirmed (" << cfg.hardware.axesLabel()
|
||
<< ")";
|
||
return true;
|
||
case PresenceAction::WarnOnly:
|
||
LOG_WARN << "hardware: " << v.reason;
|
||
return true;
|
||
case PresenceAction::Refuse:
|
||
LOG_ERROR << "hardware: " << v.reason;
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
std::optional<DumpData> freshDump(std::chrono::milliseconds wait) {
|
||
using namespace std::chrono_literals;
|
||
motor->sendCommand("DUMP");
|
||
const auto deadline = std::chrono::steady_clock::now() + wait;
|
||
while (running && std::chrono::steady_clock::now() < deadline) {
|
||
std::this_thread::sleep_for(100ms);
|
||
publishSnapshot();
|
||
DumpData dd = parseDump(motor->lastDump());
|
||
if (dd.valid) return dd;
|
||
}
|
||
return std::nullopt;
|
||
}
|
||
|
||
void applyHomedTravel(const DumpData* pre = nullptr) {
|
||
using namespace std::chrono_literals;
|
||
|
||
std::optional<DumpData> got;
|
||
if (!pre) {
|
||
got = freshDump(3s);
|
||
if (!got) {
|
||
LOG_WARN << "homed travel: no firmware dump; keeping the previous degree limits";
|
||
return;
|
||
}
|
||
}
|
||
const DumpData& dd = pre ? *pre : *got;
|
||
|
||
bool any = false;
|
||
for (const auto& a : dd.axes) {
|
||
if (a.lim_pos == a.lim_neg) continue; // that axis has not homed
|
||
const bool is_yaw = (a.axis == 'Y');
|
||
AxisMap& map = is_yaw ? cfg.geometry.yaw : cfg.geometry.pitch;
|
||
const char* name = is_yaw ? "yaw" : "pitch";
|
||
const long want_margin =
|
||
is_yaw ? cfg.yaw_soft_margin_counts : cfg.pitch_soft_margin_counts;
|
||
|
||
// The centre is arbitrary, but it has to be big enough that the whole
|
||
// travel stays positive: the range is centre +/- half the travel, so
|
||
// a centre below half-travel puts the negative end under zero. The
|
||
// 500000 default was sized for the Delta's ~89k half-travel and does
|
||
// not survive a gear ratio 7x higher.
|
||
const auto fits = checkTravelFitsPositive(name, map.center_count,
|
||
(a.lim_pos - a.lim_neg) / 2);
|
||
if (fits.ran && !fits.pass) LOG_WARN << fits.detail;
|
||
|
||
// One encoder quantum of slack: ENC_CONST makes X_ENC advance in 12.8
|
||
// microstep steps, so the midpoint can land a count or so off.
|
||
const long mid = a.lim_neg + (a.lim_pos - a.lim_neg) / 2;
|
||
if (std::labs(mid - map.center_count) > kEncoderQuantumCounts) {
|
||
LOG_ERROR << "homed " << name << " midpoint is " << mid << " but ["
|
||
<< "Motor] " << name << "_center_count is " << map.center_count
|
||
<< " -- the firmware did not adopt CENTER (is it older than this "
|
||
"host?). Refusing to derive travel limits; headings would be "
|
||
"wrong.";
|
||
continue;
|
||
}
|
||
|
||
long margin = a.soft_margin;
|
||
if (margin <= 0) {
|
||
LOG_WARN << "homed " << name
|
||
<< ": firmware reported no soft_margin (update firmware); "
|
||
"anchoring at the hard homing limit instead";
|
||
margin = 0;
|
||
} else if (margin != want_margin) {
|
||
// Trust the firmware: it is what is actually clamping the axis, so
|
||
// deriving from our value would advertise travel it will not allow.
|
||
LOG_WARN << "homed " << name << ": firmware soft_margin " << margin
|
||
<< " != configured " << want_margin
|
||
<< "; using the firmware's value";
|
||
}
|
||
|
||
const long lo_soft = a.lim_neg + margin;
|
||
const long hi_soft = a.lim_pos - margin;
|
||
const double d_lo = map.toDeg(lo_soft);
|
||
const double d_hi = map.toDeg(hi_soft);
|
||
map.min_deg = std::min(d_lo, d_hi);
|
||
map.max_deg = std::max(d_lo, d_hi);
|
||
any = true;
|
||
|
||
if (is_yaw)
|
||
LOG_INFO << "homed yaw travel: " << map.min_deg << ".." << map.max_deg
|
||
<< " deg (headings " << wrap360(map.min_deg) << ".."
|
||
<< wrap360(map.max_deg) << " true), centre " << map.center_count
|
||
<< " = " << map.deg_at_center << " deg";
|
||
else
|
||
LOG_INFO << "homed pitch travel: " << map.min_deg << ".." << map.max_deg
|
||
<< " deg, centre " << map.center_count << " = " << map.deg_at_center
|
||
<< " deg";
|
||
}
|
||
if (!any) return;
|
||
if (scheduler) scheduler->setGeometry(cfg.geometry); // re-target the scan with it
|
||
auditScanReachability();
|
||
}
|
||
|
||
// Report scan waypoints that fall in the yaw arc the gimbal cannot reach, once
|
||
// per homing rather than once per pass. The scheduler skips them at run time;
|
||
// without this the only symptom would be a grid quietly shorter than the CSV.
|
||
void auditScanReachability() {
|
||
if (grid.empty()) return;
|
||
std::size_t bad = 0;
|
||
for (const auto& p : grid.points())
|
||
if (!cfg.geometry.yaw.headingToCounts(p.yaw_deg)) ++bad;
|
||
if (bad == 0) return;
|
||
LOG_WARN << bad << " of " << grid.size()
|
||
<< " scan waypoints lie outside the reachable yaw arc ("
|
||
<< wrap360(cfg.geometry.yaw.min_deg) << ".."
|
||
<< wrap360(cfg.geometry.yaw.max_deg) << " true) and will be skipped";
|
||
}
|
||
|
||
// `camera reset`: reboot every camera (DeviceReset) and put it back on its
|
||
// profile. For a stream that has stopped delivering and will not restart -
|
||
// the state tower 111's camera was in on 2026-09-24, which only this cleared.
|
||
void resetCameras() {
|
||
const bool was = scheduler->captureActive();
|
||
stopCapture();
|
||
for (int i = 0; i < camera->cameraCount(); ++i)
|
||
LOG_INFO << "camera " << i << (camera->resetCamera(i) ? ": reset, back on its profile"
|
||
: ": reset FAILED (see above)");
|
||
if (was) startCapture();
|
||
}
|
||
|
||
void startCapture() {
|
||
// Heading setup owns the gimbal by hand. Starting a scan under it would
|
||
// move the axis between a sighting and the bearing being typed for it,
|
||
// silently poisoning the fit with a position nobody looked through.
|
||
if (heading_setup_active_) {
|
||
LOG_WARN << "Capture not started: heading setup in progress "
|
||
"(finish it, or 'gimbal heading cancel')";
|
||
return;
|
||
}
|
||
// The scheduler arms regardless, but a scan can't progress until the axes
|
||
// are homed: before READY the firmware leaves coils disabled and clamps
|
||
// MOVE targets to a coarse bench guard, so no waypoint is ever reached and
|
||
// the move/settle cycle stalls silently. Refuse with a clear message
|
||
// instead. (Single-axis builds report no P: segment, so skip pitch.)
|
||
MotorTelemetry t = motor->telemetry();
|
||
if (!t.yaw.ready() || (t.pitch_present && !t.pitch.ready())) {
|
||
LOG_WARN << "Capture not started: gimbal not homed (run init/home first)";
|
||
return;
|
||
}
|
||
camera->start();
|
||
scheduler->setCaptureActive(true);
|
||
LOG_INFO << "Capture started";
|
||
}
|
||
void stopCapture() {
|
||
camera->stop();
|
||
scheduler->setCaptureActive(false);
|
||
LOG_INFO << "Capture stopped";
|
||
}
|
||
|
||
// `gimbal heading fit <counts>:<deg> ...` / `gimbal heading offset <deg>`.
|
||
//
|
||
// `fit` is what the TUI wizard emits after the operator has sighted landmarks,
|
||
// but it is a first-class command in its own right: on a headless tower an
|
||
// operator who already knows two (encoder position, true bearing) pairs can
|
||
// type it directly.
|
||
void handleHeading(const std::vector<std::string>& tok) {
|
||
const std::string what = tok.size() > 2 ? lowerStr(tok[2]) : "";
|
||
if (what == "setup") {
|
||
// The TUI drives the interaction (arrow keys and typed bearings exist
|
||
// only there); this just claims the gimbal so a scan cannot start
|
||
// underneath it and move the axis between a sighting and its bearing.
|
||
if (procedureBusy()) {
|
||
LOG_WARN << "gimbal heading setup: " << procedureName() << " in progress";
|
||
return;
|
||
}
|
||
if (!requireManual("gimbal heading setup")) return;
|
||
heading_setup_active_ = true;
|
||
LOG_INFO << "heading setup: aim at a landmark whose true bearing you know, "
|
||
"Space to record an image, Enter to enter its bearing. Two "
|
||
"landmarks minimum, further apart is better.";
|
||
return;
|
||
}
|
||
if (what == "cancel") {
|
||
if (heading_setup_active_) LOG_INFO << "heading setup cancelled";
|
||
heading_setup_active_ = false;
|
||
return;
|
||
}
|
||
if (what == "offset") {
|
||
double off = 0;
|
||
if (tok.size() < 4) { LOG_WARN << "usage: gimbal heading offset <deg>"; return; }
|
||
try { off = std::stod(tok[3]); } catch (...) {
|
||
LOG_WARN << "gimbal heading offset: bad number '" << tok[3] << "'";
|
||
return;
|
||
}
|
||
AxisMap& y = cfg.geometry.yaw;
|
||
const double old = y.deg_at_center;
|
||
y.deg_at_center = wrap360(old + off);
|
||
applyYawMapChange();
|
||
LOG_INFO << "yaw heading at centre " << old << " -> " << y.deg_at_center
|
||
<< " deg true (offset " << off << ")";
|
||
return;
|
||
}
|
||
if (what != "fit") {
|
||
LOG_WARN << "usage: gimbal heading <setup|fit <counts>:<deg> ...|offset <deg>>";
|
||
return;
|
||
}
|
||
|
||
std::vector<HeadingSighting> sightings;
|
||
std::string err;
|
||
if (!parseHeadingSightings({tok.begin() + 3, tok.end()}, sightings, err)) {
|
||
LOG_WARN << "gimbal heading fit: " << err;
|
||
return;
|
||
}
|
||
const HeadingFitResult r = fitHeading(sightings, cfg.geometry.yaw.center_count);
|
||
if (!r.ok) {
|
||
LOG_WARN << "gimbal heading fit: " << r.error;
|
||
return;
|
||
}
|
||
|
||
AxisMap& y = cfg.geometry.yaw;
|
||
LOG_INFO << "yaw heading fit over " << r.n << " sightings spanning " << r.span_deg
|
||
<< " deg: counts_per_deg " << y.counts_per_deg << " -> " << r.counts_per_deg
|
||
<< ", heading at centre " << y.deg_at_center << " -> " << r.deg_at_center
|
||
<< " deg true (R^2=" << r.r2 << ")";
|
||
y.counts_per_deg = r.counts_per_deg;
|
||
y.deg_at_center = r.deg_at_center;
|
||
applyYawMapChange();
|
||
|
||
// The landmark wizard and a bare `gimbal heading fit` land here alike; name
|
||
// whichever it was before the flag is cleared.
|
||
calib_save_source_ = heading_setup_active_ ? "gimbal heading setup"
|
||
: "gimbal heading fit";
|
||
heading_setup_active_ = false;
|
||
// Offer to persist, through the same activity-strip prompt the pitch setup
|
||
// uses - the operator answers y/n without leaving the dashboard.
|
||
if (!opts.config_path.empty()) calib_save_pending_ = true;
|
||
}
|
||
|
||
// Re-derive everything that depends on the yaw map after it changes: the
|
||
// reachable degree window (which is in degrees, so it moves with the map) and
|
||
// the scheduler's copy used to target scan waypoints.
|
||
void applyYawMapChange() {
|
||
applyHomedTravel();
|
||
if (scheduler) scheduler->setGeometry(cfg.geometry);
|
||
}
|
||
|
||
// Capture one image now, outside any scan: the operator's reference shot while
|
||
// aiming by hand. It goes through the scheduler rather than poking the camera
|
||
// directly so it produces exactly what a scan waypoint produces - same file,
|
||
// same CamEvent on MQTT.
|
||
void takeSnapshot() {
|
||
if (!requireManual("snapshot")) return;
|
||
if (!camera || !scheduler) { LOG_WARN << "snapshot: no camera configured"; return; }
|
||
MotorTelemetry t = motor->telemetry();
|
||
if (!t.yaw.ready() || (t.pitch_present && !t.pitch.ready())) {
|
||
LOG_WARN << "snapshot: gimbal not homed (press h / run 'gimbal home' first)";
|
||
return;
|
||
}
|
||
// The camera is stopped outside a scan; leave it running afterwards so a
|
||
// burst of snapshots does not pay the start-up cost each time. stopCapture()
|
||
// and shutdown stop it as before.
|
||
camera->start();
|
||
scheduler->requestSnapshot();
|
||
LOG_INFO << "snapshot requested";
|
||
}
|
||
|
||
// Manual mode is simply "no scan running" - derived, never a separate flag an
|
||
// operator could strand. Hand aiming and a scan are mutually exclusive: both
|
||
// drive the same axes, and a nudge mid-sweep would silently corrupt the
|
||
// waypoint the next frame is published under.
|
||
bool manualMode() const { return !scheduler || !scheduler->captureActive(); }
|
||
|
||
// Guard for the hand-aiming commands. Returns false (having said why) when a
|
||
// scan owns the gimbal.
|
||
bool requireManual(const char* what) {
|
||
if (manualMode()) return true;
|
||
LOG_WARN << "ignored '" << what << "': a scan is running (press x / type 'stop' first)";
|
||
return false;
|
||
}
|
||
|
||
// `trace off|none`, `trace all`, or `trace <cat> [on|off]` — toggle the
|
||
// verbatim wire-trace categories (serial/mqtt/camera/control).
|
||
void handleTrace(const Command& c) {
|
||
if (c.device.empty() || c.device == "off" || c.device == "none") {
|
||
Logger::setCategories(0);
|
||
} else if (c.device == "all") {
|
||
Logger::setCategories(static_cast<unsigned>(LogCat::All));
|
||
} else {
|
||
bool ok = true;
|
||
LogCat cat = Logger::catFromString(c.device, &ok);
|
||
if (!ok) {
|
||
LOG_WARN << "unknown trace category: " << c.device;
|
||
return;
|
||
}
|
||
bool on = (c.option != "off"); // "trace serial" / "trace serial on" => enable
|
||
if (on) Logger::enableCategory(cat);
|
||
else Logger::disableCategory(cat);
|
||
}
|
||
LOG_INFO << "trace categories: " << traceNames(Logger::categories());
|
||
}
|
||
|
||
// `goto <yaw_deg> <pitch_deg>` — aim the gimbal at an absolute heading and
|
||
// elevation in degrees. Converts to encoder counts via the operator-calibrated
|
||
// Geometry maps (which soft-clamp to the travel limits) and issues a two-axis
|
||
// MOVE so both axes start together.
|
||
// Parse "<a>,<b>" or "<a> <b>" (tokens from `from` onward) into two numbers.
|
||
static bool parsePair(const std::vector<std::string>& tok, size_t from,
|
||
double& a, double& b) {
|
||
std::string s;
|
||
for (size_t i = from; i < tok.size(); ++i) s += tok[i] + " ";
|
||
for (char& ch : s) if (ch == ',') ch = ' ';
|
||
std::istringstream iss(s);
|
||
return static_cast<bool>(iss >> a >> b);
|
||
}
|
||
|
||
// Cross-check the degree scale against the travel homing just measured, and
|
||
// bootstrap it when it is obviously wrong.
|
||
//
|
||
// Called ONLY from the homing path. applyHomedTravel() runs after every
|
||
// heading fit and offset too, and deriving there would overwrite a landmark
|
||
// calibration with this rougher figure on every tweak.
|
||
void deriveCountsPerDeg(const DumpData& dd) {
|
||
struct AxisRef {
|
||
const char* name;
|
||
char axis;
|
||
const AxisDrivetrainConfig& drive;
|
||
AxisMap& map;
|
||
};
|
||
AxisRef axes[] = {
|
||
{"yaw", 'Y', cfg.drivetrain.yaw, cfg.geometry.yaw},
|
||
{"pitch", 'P', cfg.drivetrain.pitch, cfg.geometry.pitch},
|
||
};
|
||
|
||
bool changed = false;
|
||
for (auto& a : axes) {
|
||
if (a.drive.travel_deg <= 0.0) continue; // nothing declared to divide by
|
||
// Measured travel from THIS dump. Taking it from motor->lastDump()
|
||
// instead would read whatever dump happened to be cached - which,
|
||
// before applyHomedTravel() fetches a fresh one, is the pre-homing
|
||
// dump whose limits are still zero, so the derivation would skip in
|
||
// silence on every single home.
|
||
long counts = 0;
|
||
for (const auto& ax : dd.axes)
|
||
if (ax.axis == a.axis) counts = std::labs(ax.lim_pos - ax.lim_neg);
|
||
if (counts <= 0) continue; // that axis did not home
|
||
|
||
// The angle is unsigned; which way the axis counts is a property of
|
||
// the map, so carry the existing sign rather than inventing one.
|
||
const double derived =
|
||
std::copysign(counts / a.drive.travel_deg, a.map.counts_per_deg);
|
||
const double current = a.map.counts_per_deg;
|
||
const double ratio =
|
||
std::fabs(current) > 1e-9 ? std::fabs(derived / current) : 1e9;
|
||
|
||
// Only bootstrap a scale that is obviously wrong. Within a factor of
|
||
// two this is a health check, not a correction: a landmark fit is a
|
||
// better measurement than this one and must not be clobbered by a
|
||
// routine re-home.
|
||
if (ratio > 2.0 || ratio < 0.5) {
|
||
LOG_WARN << a.name << " counts_per_deg derived from homing: " << derived
|
||
<< " (was " << current << ") = " << counts << " counts / "
|
||
<< a.drive.travel_deg << " deg of travel declared in hardware.ini ["
|
||
<< "Motor." << a.name << "] travel_deg. That angle is a NAMEPLATE "
|
||
"figure: every bearing carries its error until it is measured. "
|
||
"Run 'gimbal heading setup' to supersede this with a landmark fit.";
|
||
a.map.counts_per_deg = derived;
|
||
changed = true;
|
||
} else {
|
||
LOG_INFO << a.name << " counts_per_deg " << current << " agrees with the homed "
|
||
<< "travel (" << counts << " counts / " << a.drive.travel_deg
|
||
<< " deg = " << derived << "); leaving the calibration alone";
|
||
}
|
||
}
|
||
|
||
if (changed && !opts.config_path.empty()) {
|
||
calib_save_source_ = "homed travel / hardware.ini travel_deg (nameplate)";
|
||
calib_save_pending_ = true;
|
||
}
|
||
}
|
||
|
||
// Homed physical travel (counts) for an axis: the endstop-to-endstop span from
|
||
// the last firmware dump. Returns 0 if no dump has been captured yet (caller
|
||
// should request one). Deliberately does NOT fall back to the configured degree
|
||
// range × counts_per_deg — that conflates calibration with physical travel and,
|
||
// when min/max are unset, yields an absurd span.
|
||
long homedTravelCounts(char axis) {
|
||
DumpData d = parseDump(motor->lastDump());
|
||
if (d.valid)
|
||
for (const auto& ax : d.axes)
|
||
if (ax.axis == axis) return std::labs(ax.lim_pos - ax.lim_neg);
|
||
return 0;
|
||
}
|
||
|
||
// `gimbal <subcommand>` — unified, lowercase motor control. `move` is degrees
|
||
// (default), `steps` is raw encoder counts; the rest map to firmware verbs.
|
||
void handleGimbal(const std::string& line) {
|
||
std::istringstream iss(line);
|
||
std::vector<std::string> tok;
|
||
std::string t;
|
||
while (iss >> t) tok.push_back(t); // tok[0] == "gimbal"
|
||
if (tok.size() < 2) {
|
||
LOG_WARN << "usage: gimbal <move|steps|nudge|jog|heading|pitch|home|stop|reset|"
|
||
"enable|disable|speed|current|setpos|status|dump|raw> "
|
||
"(self-test: 'test')";
|
||
return;
|
||
}
|
||
std::string sub = tok[1];
|
||
for (char& ch : sub) ch = static_cast<char>(std::tolower((unsigned char)ch));
|
||
|
||
// Hand aiming is manual-mode only: these fight a running scan for the
|
||
// same axes, and a move mid-sweep mislabels the next published frame.
|
||
if (sub == "move" || sub == "steps" || sub == "nudge" || sub == "jog") {
|
||
if (!requireManual(("gimbal " + sub).c_str())) return;
|
||
}
|
||
|
||
if (sub == "move") {
|
||
double yaw_deg = 0, pitch_deg = 0;
|
||
if (!parsePair(tok, 2, yaw_deg, pitch_deg)) {
|
||
LOG_WARN << "usage: gimbal move <yaw_deg>,<pitch_deg>";
|
||
return;
|
||
}
|
||
long yc = cfg.geometry.yaw.toCounts(yaw_deg);
|
||
long pc = cfg.geometry.pitch.toCounts(pitch_deg);
|
||
LOG_INFO << "gimbal move yaw=" << yaw_deg << " pitch=" << pitch_deg
|
||
<< " deg -> MOVE " << yc << "," << pc;
|
||
motor->sendCommand("MOVE " + std::to_string(yc) + "," + std::to_string(pc));
|
||
} else if (sub == "steps") {
|
||
double yc = 0, pc = 0;
|
||
if (!parsePair(tok, 2, yc, pc)) {
|
||
LOG_WARN << "usage: gimbal steps <yaw_counts>,<pitch_counts>";
|
||
return;
|
||
}
|
||
motor->sendCommand("MOVE " + std::to_string((long)yc) + "," + std::to_string((long)pc));
|
||
} else if (sub == "nudge") {
|
||
if (tok.size() < 4) { LOG_WARN << "usage: gimbal nudge <yaw|pitch> <+/-pct>"; return; }
|
||
char axis = (tok[2][0] == 'p' || tok[2][0] == 'P') ? 'P' : 'Y';
|
||
double pct = 0;
|
||
try { pct = std::stod(tok[3]); } catch (...) { LOG_WARN << "gimbal nudge: bad percent"; return; }
|
||
// Nudge is a fraction of the *homed* physical travel (endstop-to-endstop
|
||
// from the firmware dump), so it is independent of calibration and never
|
||
// a fraction of the configured degree clamps. If no dump is available
|
||
// yet, request one and bail rather than guessing.
|
||
long range = homedTravelCounts(axis);
|
||
if (range <= 0) {
|
||
LOG_WARN << "gimbal nudge: homed travel unknown; home or 'gimbal dump' first";
|
||
motor->sendCommand("DUMP");
|
||
return;
|
||
}
|
||
MotorTelemetry tel = motor->telemetry();
|
||
long cur = (axis == 'Y') ? tel.yaw.xenc : tel.pitch.xenc;
|
||
long target = cur + static_cast<long>(pct / 100.0 * range);
|
||
LOG_INFO << "gimbal nudge " << axis << " " << pct << "% -> MOVE " << axis << " " << target;
|
||
motor->sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
|
||
} else if (sub == "jog") {
|
||
// Relative move in raw counts. `nudge` is a percent of travel and
|
||
// `steps` is absolute; neither can express the sub-degree steps the
|
||
// heading wizard needs, and counts are what the encoder resolves.
|
||
if (tok.size() < 4) { LOG_WARN << "usage: gimbal jog <yaw|pitch> <+/-counts>"; return; }
|
||
char axis = (tok[2][0] == 'p' || tok[2][0] == 'P') ? 'P' : 'Y';
|
||
long delta = 0;
|
||
try { delta = std::stol(tok[3]); } catch (...) { LOG_WARN << "gimbal jog: bad count"; return; }
|
||
MotorTelemetry tel = motor->telemetry();
|
||
long cur = (axis == 'Y') ? tel.yaw.xenc : tel.pitch.xenc;
|
||
long target = cur + delta;
|
||
LOG_INFO << "gimbal jog " << axis << " " << delta << " counts -> MOVE " << axis
|
||
<< " " << target;
|
||
motor->sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
|
||
} else if (sub == "heading") {
|
||
handleHeading(tok);
|
||
} else if (sub == "pitch") {
|
||
if (tok.size() > 2 && lowerStr(tok[2]) == "setup") startPitchSetup();
|
||
else LOG_WARN << "usage: gimbal pitch setup";
|
||
} else if (sub == "dump") {
|
||
LOG_INFO << "requesting firmware dump...";
|
||
motor->sendCommand("DUMP");
|
||
} else if (sub == "raw") {
|
||
// Low-level escape hatch: send the text inside the quotation marks to
|
||
// the serial port verbatim (the transport appends the newline). This
|
||
// exposes any firmware verb directly, including ones with no wrapper.
|
||
std::string raw;
|
||
auto q1 = line.find('"');
|
||
auto q2 = line.rfind('"');
|
||
if (q1 != std::string::npos && q2 != std::string::npos && q2 > q1) {
|
||
raw = line.substr(q1 + 1, q2 - q1 - 1); // exact text between quotes
|
||
} else {
|
||
for (size_t i = 2; i < tok.size(); ++i) // unquoted: join remaining tokens
|
||
raw += (raw.empty() ? "" : " ") + tok[i];
|
||
}
|
||
if (raw.empty()) {
|
||
LOG_WARN << "usage: gimbal raw \"<verbatim firmware command>\"";
|
||
return;
|
||
}
|
||
LOG_INFO << "gimbal raw -> " << raw;
|
||
motor->sendCommand(raw);
|
||
} else if (sub == "home" && tok.size() <= 2) {
|
||
// Full re-home: enable + HOME + wait for READY + re-anchor the yaw zero
|
||
// (same path as startup --init), so the homing key behaves like a boot
|
||
// home rather than a fire-and-forget HOME that skips the re-anchor.
|
||
runInitSequence();
|
||
} else if (sub == "home" || sub == "stop" || sub == "reset" || sub == "enable" ||
|
||
sub == "disable" || sub == "speed" || sub == "setpos" ||
|
||
sub == "status" || sub == "current") {
|
||
// Passthrough to the (case-insensitive) firmware verb + args.
|
||
std::string fw = sub;
|
||
for (char& ch : fw) ch = static_cast<char>(std::toupper((unsigned char)ch));
|
||
for (size_t i = 2; i < tok.size(); ++i) fw += " " + tok[i];
|
||
motor->sendCommand(fw);
|
||
} else {
|
||
LOG_WARN << "unknown gimbal subcommand: " << sub;
|
||
}
|
||
}
|
||
|
||
void startPitchSetup() {
|
||
// Pitch is the one axis that genuinely needs the IMU: level is defined by
|
||
// gravity, and nothing else on the tower can measure it. Yaw does not -
|
||
// `gimbal heading setup` uses landmarks instead.
|
||
if (!imu) {
|
||
LOG_WARN << "gimbal pitch setup requires the IMU (declare [Hardware] imu); "
|
||
"yaw does not - use 'gimbal heading setup'";
|
||
return;
|
||
}
|
||
if (calib && calib->running()) {
|
||
LOG_WARN << "pitch setup already running (press Esc, or type 'stop', to cancel)";
|
||
return;
|
||
}
|
||
if (heading_setup_active_) {
|
||
LOG_WARN << "pitch setup: heading setup in progress ('gimbal heading cancel' first)";
|
||
return;
|
||
}
|
||
stopCapture(); // free the serial link + don't fight the routine's moves
|
||
calib = std::make_unique<PitchSetupRoutine>(*motor, *imu, cfg.geometry);
|
||
calib->start();
|
||
}
|
||
|
||
// Persist the live (just-calibrated) [Motor] geometry back into config.ini.
|
||
// Superseded values are kept above their key as `;prev` comments, stamped with
|
||
// when and what replaced them, so a calibration that turns out worse than the
|
||
// one it replaced can be reverted by uncommenting.
|
||
void saveCalibration() {
|
||
if (!calib_save_pending_) return;
|
||
calib_save_pending_ = false;
|
||
const std::string provenance =
|
||
utcStampNow() + ", " +
|
||
(calib_save_source_.empty() ? std::string("calibration") : calib_save_source_);
|
||
if (saveMotorGeometry(opts.config_path, cfg.geometry, cfg.yaw_soft_margin_counts,
|
||
cfg.pitch_soft_margin_counts, provenance))
|
||
LOG_INFO << "[Motor] calibration saved as the new default in " << opts.config_path
|
||
<< " (previous values kept as ';prev' comments)";
|
||
else
|
||
LOG_WARN << "could not write [Motor] calibration to " << opts.config_path;
|
||
}
|
||
|
||
void cancelPitchSetup() {
|
||
if (calib && calib->running()) {
|
||
LOG_INFO << "cancelling pitch setup";
|
||
calib->cancel();
|
||
motor->sendCommand("STOP ALL");
|
||
}
|
||
}
|
||
|
||
// ---- Procedure lock-out (test / pitch setup / homing are mutually exclusive) ----
|
||
enum class Procedure { None, Homing, PitchSetup, Test };
|
||
Procedure activeProcedure() const {
|
||
if (tester && tester->running()) return Procedure::Test;
|
||
if (calib && calib->running()) return Procedure::PitchSetup;
|
||
if (homing_active_.load()) return Procedure::Homing;
|
||
return Procedure::None;
|
||
}
|
||
bool procedureBusy() const { return activeProcedure() != Procedure::None; }
|
||
const char* procedureName() const {
|
||
switch (activeProcedure()) {
|
||
case Procedure::Test: return "test";
|
||
case Procedure::PitchSetup: return "pitch setup";
|
||
case Procedure::Homing: return "homing";
|
||
default: return "none";
|
||
}
|
||
}
|
||
|
||
void cancelActiveProcedure() {
|
||
switch (activeProcedure()) {
|
||
case Procedure::Test:
|
||
LOG_INFO << "cancelling test";
|
||
tester->cancel();
|
||
motor->sendCommand("STOP ALL");
|
||
break;
|
||
case Procedure::PitchSetup:
|
||
cancelPitchSetup();
|
||
break;
|
||
case Procedure::Homing:
|
||
LOG_INFO << "cancelling homing";
|
||
cancel_procedure_ = true;
|
||
motor->sendCommand("STOP ALL");
|
||
break;
|
||
case Procedure::None:
|
||
LOG_INFO << "nothing to cancel";
|
||
break;
|
||
}
|
||
}
|
||
|
||
// Pop the command queue and drop any interfering command (homing runs on the
|
||
// control thread, so handleCommand() is not reached during it). Used inside
|
||
// runInitSequence()'s wait loops so queued moves are *ignored*, not deferred.
|
||
void drainAndDiscardCommands() {
|
||
std::queue<std::string> local;
|
||
{
|
||
std::lock_guard<std::mutex> lock(cmd_mutex);
|
||
std::swap(local, cmd_queue);
|
||
}
|
||
while (!local.empty()) {
|
||
const std::string& line = local.front();
|
||
Command c = parseCommand(line);
|
||
if (c.verb == "cancel" || c.verb == "stop")
|
||
cancel_procedure_ = true;
|
||
else if (!c.empty())
|
||
LOG_WARN << "ignored '" << c.verb << "': homing in progress (Esc to cancel)";
|
||
local.pop();
|
||
}
|
||
}
|
||
|
||
// ---- `test` command ----
|
||
static std::string lowerStr(std::string s) {
|
||
for (char& ch : s) ch = static_cast<char>(std::tolower((unsigned char)ch));
|
||
return s;
|
||
}
|
||
|
||
void handleTest(const std::string& line) {
|
||
std::istringstream iss(line);
|
||
std::vector<std::string> tok;
|
||
std::string t;
|
||
while (iss >> t) tok.push_back(t); // tok[0] == "test"
|
||
|
||
if (tok.size() >= 2) {
|
||
std::string sub = lowerStr(tok[1]);
|
||
if (sub == "baseline") { promoteBaseline(); return; }
|
||
if (sub == "list") { listTestProfiles(); return; }
|
||
}
|
||
|
||
// Split [subsystem] [test] [profile]: a token naming a configured profile
|
||
// is the profile; the remaining (ordered) tokens are subsystem then test.
|
||
std::string subsystem, leaf, profileName;
|
||
for (size_t i = 1; i < tok.size(); ++i) {
|
||
std::string w = lowerStr(tok[i]);
|
||
if (profileName.empty() && cfg.test.profiles.count(w)) { profileName = w; continue; }
|
||
if (subsystem.empty()) subsystem = w;
|
||
else if (leaf.empty()) leaf = w;
|
||
}
|
||
if (profileName.empty()) profileName = cfg.test.default_profile;
|
||
|
||
uint32_t mask = 0;
|
||
std::string err;
|
||
if (!resolveTestSelection(subsystem, leaf, mask, err)) {
|
||
LOG_WARN << "test: " << err;
|
||
LOG_INFO << "usage: test [gimbal|imu|host [<leaf>]] [<profile>] (also: test list, test baseline)";
|
||
return;
|
||
}
|
||
startTest(mask, profileName);
|
||
}
|
||
|
||
void startTest(uint32_t mask, const std::string& profileName) {
|
||
if (procedureBusy()) {
|
||
LOG_WARN << "test: " << procedureName() << " already in progress (Esc to cancel)";
|
||
return;
|
||
}
|
||
// IMU leaves need an IMU; drop them (with a warning) if absent.
|
||
if ((mask & T_NEEDS_IMU) && !imu) {
|
||
LOG_WARN << "test: IMU tests skipped (declare [Hardware] imu)";
|
||
mask &= ~T_NEEDS_IMU;
|
||
}
|
||
// Motor leaves need a homed gimbal.
|
||
if (mask & T_NEEDS_MOTOR) {
|
||
MotorTelemetry tel = motor->telemetry();
|
||
if (!tel.yaw.ready() && !(tel.pitch_present && tel.pitch.ready())) {
|
||
LOG_WARN << "test: gimbal not READY (home first)";
|
||
return;
|
||
}
|
||
}
|
||
if (mask == 0) { LOG_WARN << "test: nothing to run"; return; }
|
||
|
||
TestProfile profile;
|
||
if (const TestProfile* p = cfg.test.find(profileName)) profile = *p;
|
||
else profile.name = profileName; // unknown name => built-in defaults
|
||
test_drift_warn_ = profile.get("drift_warn_pct", 15.0);
|
||
test_drift_gate_ = profile.getBool("drift_gates_pass", false);
|
||
|
||
// Paused, not ended: a test run on a tower in service (install-tower
|
||
// --verify does exactly that) must not leave it idle afterwards.
|
||
resume_capture_after_test_ = scheduler->captureActive();
|
||
stopCapture(); // free the serial link; don't fight the routine's moves
|
||
LOG_INFO << "starting hardware test (profile '" << profile.name << "')";
|
||
// What `test hardware` needs beyond the motor and IMU: the declaration to
|
||
// check against, and the subsystems that answer for themselves.
|
||
HardwareTestContext hw;
|
||
hw.declared = &cfg.hardware;
|
||
hw.drive = &cfg.drivetrain;
|
||
hw.env = env.get();
|
||
hw.camera = camera.get();
|
||
for (const auto* c : cfg.camera.active()) {
|
||
hw.configured_cameras.push_back(c->name);
|
||
hw.frame_timeout_ms.push_back(c->timingFor(c->profile).frame_timeout_ms);
|
||
}
|
||
hw.i2c_device = cfg.env.i2c_device;
|
||
hw.geo_yaw = cfg.geometry.yaw;
|
||
hw.geo_pitch = cfg.geometry.pitch;
|
||
hw.usbfs_min_mb = cfg.test.usbfs_min_mb;
|
||
hw.lpm_path = cfg.test.usb3_lpm_path;
|
||
|
||
tester = std::make_unique<TestRunner>(*motor, imu.get(), cfg.geometry, mask, profile,
|
||
cfg.paths.output_dir, std::move(hw));
|
||
tester->start();
|
||
}
|
||
|
||
void listTestProfiles() {
|
||
LOG_INFO << "test profiles (default: " << cfg.test.default_profile << "):";
|
||
if (cfg.test.profiles.empty()) LOG_INFO << " (none configured; built-in defaults used)";
|
||
for (const auto& [name, p] : cfg.test.profiles)
|
||
LOG_INFO << " " << name << " (" << p.params.size() << " params)";
|
||
}
|
||
|
||
std::string baselinePath() const {
|
||
return cfg.test.baseline_file.empty()
|
||
? (paths::defaultLogDir() + "/test_baseline.txt")
|
||
: cfg.test.baseline_file;
|
||
}
|
||
|
||
void promoteBaseline() {
|
||
if (last_test_report_path.empty()) {
|
||
LOG_WARN << "test baseline: no test report from this session to promote";
|
||
return;
|
||
}
|
||
std::error_code ec;
|
||
std::filesystem::copy_file(last_test_report_path, baselinePath(),
|
||
std::filesystem::copy_options::overwrite_existing, ec);
|
||
if (ec) LOG_WARN << "test baseline: copy failed: " << ec.message();
|
||
else LOG_INFO << "test baseline set from " << last_test_report_path << " -> " << baselinePath();
|
||
}
|
||
|
||
// Read+parse a report file; returns valid()==false-ish empty report on failure.
|
||
static bool loadReport(const std::string& path, TestReport& out) {
|
||
std::ifstream f(path);
|
||
if (!f) return false;
|
||
std::ostringstream ss;
|
||
ss << f.rdbuf();
|
||
out = parseTestReport(ss.str());
|
||
return !out.sections.empty();
|
||
}
|
||
|
||
// Most recent prior "test_*.log" in the log dir (excluding `exclude`).
|
||
std::string latestPriorTestReport(const std::string& exclude) const {
|
||
namespace fs = std::filesystem;
|
||
std::string best;
|
||
std::error_code ec;
|
||
const std::string dir = paths::defaultLogDir();
|
||
if (!fs::exists(dir, ec)) return best;
|
||
for (auto& e : fs::directory_iterator(dir, ec)) {
|
||
const std::string name = e.path().filename().string();
|
||
if (name.rfind("test_", 0) != 0 || e.path().extension() != ".log") continue;
|
||
if (e.path().string() == exclude) continue;
|
||
if (best.empty() || name > fs::path(best).filename().string())
|
||
best = e.path().string();
|
||
}
|
||
return best;
|
||
}
|
||
|
||
// Pick up results produced by background work (calibration thread, DIAG
|
||
// capture) — runs on the control thread each tick so all geometry/state
|
||
// mutation stays single-threaded.
|
||
void pollBackgroundResults() {
|
||
if (calib) {
|
||
if (auto g = calib->takeResult()) {
|
||
cfg.geometry = *g;
|
||
if (scheduler) scheduler->setGeometry(cfg.geometry); // also fix scan targeting
|
||
LOG_INFO << "calibration applied to live geometry";
|
||
// Capture the structured report for the activity strip + gimbal view.
|
||
CalibReport rep = calib->report();
|
||
CalibResultView v;
|
||
v.has = rep.valid;
|
||
v.ts_ms = rep.ts_ms;
|
||
last_calib_summary.clear();
|
||
for (const auto& ax : rep.axes) {
|
||
CalibAxisView* dst = (ax.axis == 'P') ? &v.pitch : &v.yaw;
|
||
if (ax.axis == 'P') v.pitch_present = true;
|
||
dst->ok = ax.ok;
|
||
dst->counts_per_deg = ax.counts_per_deg;
|
||
dst->deg_at_center = ax.deg_at_center;
|
||
dst->r2 = ax.r2;
|
||
dst->n = ax.n;
|
||
char b[96];
|
||
std::snprintf(b, sizeof(b), "%s cpd=%.3f deg@ctr=%.3f R2=%.4f (n=%d)%s",
|
||
ax.axis == 'P' ? "PITCH" : "YAW", ax.counts_per_deg,
|
||
ax.deg_at_center, ax.r2, ax.n, ax.ok ? "" : " FIT FAILED");
|
||
last_calib_summary.emplace_back(b);
|
||
}
|
||
last_calib_view = v;
|
||
last_calib_ts = nowEpochMs();
|
||
// Offer to persist it as the new default (answered via the activity strip).
|
||
calib_save_source_ = "gimbal pitch setup";
|
||
if (rep.valid && !opts.config_path.empty()) calib_save_pending_ = true;
|
||
}
|
||
}
|
||
if (tester) {
|
||
if (auto rep = tester->takeReport()) {
|
||
TestReport r = std::move(*rep);
|
||
// Compare against the pinned baseline and the most recent prior run.
|
||
TestReport baseline, prev;
|
||
bool have_baseline = loadReport(baselinePath(), baseline);
|
||
bool have_prev = loadReport(latestPriorTestReport(""), prev);
|
||
applyComparison(r, have_baseline ? &baseline : nullptr,
|
||
have_prev ? &prev : nullptr, test_drift_warn_, test_drift_gate_);
|
||
|
||
std::string text = formatTestReport(r);
|
||
std::string path = paths::writeLogFile(paths::timestampedLogName("test"), text);
|
||
if (!path.empty()) {
|
||
last_test_report_path = path;
|
||
LOG_INFO << "test report written: " << path;
|
||
}
|
||
// Surface a concise per-section summary in the log + activity strip.
|
||
last_test_summary.clear();
|
||
for (const auto& s : r.sections) {
|
||
if (!s.ran) continue;
|
||
std::string l = s.id + ": " + (s.pass ? "PASS" : "FAIL");
|
||
last_test_summary.push_back(l);
|
||
LOG_INFO << "test " << l;
|
||
}
|
||
LOG_INFO << "test overall: " << (r.all_pass ? "PASS" : "FAIL")
|
||
<< " (profile " << r.profile << ")";
|
||
last_test_pass = r.all_pass;
|
||
last_test_ts = nowEpochMs();
|
||
last_test_view = TestResultView{};
|
||
last_test_view.has = true;
|
||
last_test_view.ts_ms = last_test_ts;
|
||
last_test_view.pass = r.all_pass;
|
||
last_test_view.profile = r.profile;
|
||
last_test_view.summary = last_test_summary;
|
||
if (!opts.run_test.empty()) {
|
||
exit_code_ = r.all_pass ? 0 : kExitTestFail;
|
||
running = false;
|
||
}
|
||
if (resume_capture_after_test_) {
|
||
resume_capture_after_test_ = false;
|
||
LOG_INFO << "resuming capture, paused for the test";
|
||
startCapture();
|
||
}
|
||
}
|
||
}
|
||
unsigned seq = motor->diagSeq();
|
||
if (seq != last_diag_seq) {
|
||
last_diag_seq = seq;
|
||
diag_running_ = false;
|
||
DiagResult dr = parseDiag(motor->lastDiag());
|
||
std::ostringstream summary;
|
||
for (const auto& l : formatDiag(dr)) { LOG_INFO << l; summary << l << "\n"; }
|
||
// Concise per-axis lines for the activity strip + the per-axis worst
|
||
// tracking error for the gimbal view. Full detail is logged above.
|
||
last_diag_summary.clear();
|
||
last_diag_view = DiagResultView{};
|
||
last_diag_view.has = dr.valid;
|
||
for (const auto& ax : dr.axes) {
|
||
DiagAxisView dv;
|
||
dv.axis = ax.axis;
|
||
dv.has = !ax.tests.empty();
|
||
dv.pass = ax.pass;
|
||
dv.err_peak = dv.err_rms = dv.err_still = -1; // -1 stays if no encoder
|
||
for (const auto& tst : ax.tests) { // worst (max) across the tests
|
||
if (tst.err_peak < 0) continue; // no-encoder test: skip
|
||
dv.err_peak = std::max(dv.err_peak, tst.err_peak);
|
||
dv.err_rms = std::max(dv.err_rms, tst.err_rms);
|
||
dv.err_still = std::max(dv.err_still, tst.err_still);
|
||
}
|
||
last_diag_view.axes.push_back(dv);
|
||
std::string line = std::string(1, ax.axis) + ": " +
|
||
(ax.pass ? "PASS" : "FAIL");
|
||
if (dv.has && dv.err_peak >= 0) // -1 == no encoder
|
||
line += " peak " + std::to_string(dv.err_peak) +
|
||
" still " + std::to_string(dv.err_still);
|
||
line += " (" + std::to_string(ax.tests.size()) + " tests)";
|
||
last_diag_summary.push_back(line);
|
||
}
|
||
last_diag_pass = dr.allPass();
|
||
last_diag_ts = nowEpochMs();
|
||
last_diag_view.ts_ms = last_diag_ts;
|
||
std::string path = paths::writeLogFile(
|
||
paths::timestampedLogName("diag"),
|
||
motor->lastDiag() + "\n--- parsed ---\n" + summary.str());
|
||
if (!path.empty()) LOG_INFO << "diagnostics log written: " << path;
|
||
}
|
||
// Safety: clear a stuck diag flag if the firmware never replied.
|
||
if (diag_running_ &&
|
||
std::chrono::steady_clock::now() - diag_started_ > std::chrono::seconds(30))
|
||
diag_running_ = false;
|
||
}
|
||
|
||
// Commands that would disturb a running procedure (motion, capture, a second
|
||
// long op, or a DUMP over the same serial link). Ignored while busy.
|
||
static bool interferingVerb(const std::string& verb) {
|
||
return verb == "gimbal" || verb == "start" || verb == "test" ||
|
||
verb == "refresh" || verb == "snapshot" || verb == "camera";
|
||
}
|
||
|
||
void handleCommand(const std::string& line) {
|
||
Command c = parseCommand(line);
|
||
if (c.empty()) return;
|
||
LOG_TRACE_CAT(LogCat::Control) << "cmd " << line;
|
||
|
||
// Lock-out: while a test/calibration/homing runs, ignore interfering
|
||
// commands (Esc, or `stop`/`cancel`, cancels the procedure instead).
|
||
if (procedureBusy() && interferingVerb(c.verb)) {
|
||
LOG_WARN << "ignored '" << c.verb << "': " << procedureName()
|
||
<< " in progress (Esc to cancel)";
|
||
return;
|
||
}
|
||
|
||
if (c.verb == "help") {
|
||
// c.device holds the optional topic (first token after "help").
|
||
for (const auto& l : renderHelp(c.device)) LOG_INFO << l;
|
||
} else if (c.verb == "test") {
|
||
handleTest(line);
|
||
} else if (c.verb == "cancel") {
|
||
cancelActiveProcedure();
|
||
} else if (c.verb == "gimbal") {
|
||
handleGimbal(line);
|
||
} else if (c.verb == "imu") {
|
||
handleImu(line);
|
||
} else if (c.verb == "calib") {
|
||
// Answer to the "save to config?" prompt (activity-strip y/n keys).
|
||
// Shared by `gimbal pitch setup` and `gimbal heading fit`: both change
|
||
// the [Motor] map and both offer the same choice.
|
||
if (c.device == "save") saveCalibration();
|
||
else if (c.device == "discard") {
|
||
calib_save_pending_ = false;
|
||
LOG_INFO << "kept for this session only (not saved to config)";
|
||
}
|
||
} else if (c.verb == "exit") {
|
||
running = false;
|
||
} else if (c.verb == "camera") {
|
||
if (c.device == "reset") resetCameras();
|
||
else LOG_WARN << "usage: camera reset";
|
||
} else if (c.verb == "snapshot") {
|
||
takeSnapshot();
|
||
} else if (c.verb == "start") {
|
||
startCapture();
|
||
} else if (c.verb == "stop") {
|
||
// `stop`/`x` cancels any running procedure, else stops capture.
|
||
if (procedureBusy()) cancelActiveProcedure();
|
||
else stopCapture();
|
||
} else if (c.verb == "debug") {
|
||
bool on = Logger::level() != LogLevel::Debug;
|
||
Logger::setLevel(on ? LogLevel::Debug : LogLevel::Info);
|
||
LOG_INFO << "debug logging " << (on ? "on" : "off");
|
||
} else if (c.verb == "refresh") {
|
||
// Re-read live device state surfaced in the expanded views: the IMU
|
||
// configuration (e.g. after the XKF profile was changed externally)
|
||
// and the firmware register dump shown in the gimbal 'g' view.
|
||
LOG_INFO << "refresh: re-reading IMU configuration + firmware dump";
|
||
if (imu) {
|
||
imu->refreshConfig();
|
||
imu_config_done_ = false; // force buildSnapshot to re-read config()
|
||
}
|
||
motor->sendCommand("DUMP");
|
||
} else if (c.verb == "trace") {
|
||
handleTrace(c);
|
||
} else if (c.verb == "set") {
|
||
if (c.device == "camera") {
|
||
// "set camera profile night" arrives as option="profile night":
|
||
// the parser joins non-numeric trailing tokens into `option`. With
|
||
// two words after "profile" the first names the camera.
|
||
if (c.option.rfind("profile ", 0) == 0) {
|
||
const std::string arg = c.option.substr(8);
|
||
const auto sp = arg.find(' ');
|
||
if (sp != std::string::npos)
|
||
applyProfile(arg.substr(0, sp), arg.substr(sp + 1));
|
||
else if (camera_profiles.size() == 1)
|
||
applyProfile(camera_profiles.begin()->first, arg);
|
||
else
|
||
LOG_WARN << "more than one camera configured: "
|
||
"usage: set camera profile <camera> <name>";
|
||
} else if (c.option == "profile") {
|
||
LOG_WARN << "usage: set camera profile [<camera>] <name>";
|
||
}
|
||
else if (c.option == "jxlq" && c.has_value) pipeline->setDistance(c.value);
|
||
else if (c.option == "jxle" && c.has_value) pipeline->setEffort(static_cast<int>(c.value));
|
||
else if (c.option == "display") {
|
||
const bool on = c.value != 0.0;
|
||
pipeline->setDisplay(on);
|
||
// Confirm it: the preview only repaints when a frame goes
|
||
// through the pipeline, so with no capture running there is
|
||
// nothing on screen and silence is indistinguishable from the
|
||
// command having been rejected.
|
||
LOG_INFO << "local display " << (on ? "on" : "off")
|
||
<< " (updates on the next captured frame)";
|
||
}
|
||
else LOG_WARN << "unknown 'set camera' option: " << c.option;
|
||
} else if (c.device == "fps" && c.has_value) {
|
||
scheduler->setImageRate(c.value);
|
||
LOG_INFO << "capture rate set to " << c.value << " img/s";
|
||
} else {
|
||
LOG_WARN << "unknown 'set' target: " << c.device;
|
||
}
|
||
} else {
|
||
LOG_WARN << "unknown command: " << c.verb;
|
||
}
|
||
}
|
||
|
||
// --test: start the requested test once the firmware is talking AND has been
|
||
// told its drivetrain, then let pollBackgroundResults() end the run with the
|
||
// verdict.
|
||
//
|
||
// Not immediately at startup: the firmware's NEED is answered from inside the
|
||
// control loop, so a DUMP taken on the first tick can catch an axis still in
|
||
// NEEDCFG and fail `test hardware motor` on values nobody had sent yet - a
|
||
// false alarm that would teach people to ignore the test. A deadline stops a
|
||
// dead link from waiting forever; the test then runs anyway and reports the
|
||
// real failure rather than this function inventing one.
|
||
void driveOneShotTest() {
|
||
if (opts.run_test.empty() || oneshot_started_) return;
|
||
using namespace std::chrono;
|
||
const auto now = steady_clock::now();
|
||
const MotorTelemetry tel = motor->telemetry();
|
||
auto axisSettled = [](AxisState a) {
|
||
return a == AxisState::Reset || a == AxisState::Ready || a == AxisState::Error;
|
||
};
|
||
// Pitch counts only where it is both expected and reporting: a yaw-only
|
||
// firmware has no P segment to wait for.
|
||
const AxisState st = tel.yaw.state;
|
||
const bool settled =
|
||
axisSettled(st) && (!cfg.hardware.hasAxis("pitch") || !tel.pitch_present ||
|
||
axisSettled(tel.pitch.state));
|
||
if (!settled) oneshot_link_ok_ = {};
|
||
else if (oneshot_link_ok_ == steady_clock::time_point{}) oneshot_link_ok_ = now;
|
||
|
||
const bool grace_over = settled && now - oneshot_link_ok_ >= seconds(1);
|
||
const bool timed_out = now - oneshot_since_ >= seconds(20);
|
||
if (!grace_over && !timed_out) return;
|
||
if (!grace_over)
|
||
LOG_WARN << "--test: the motor link did not settle within 20 s (an axis is still "
|
||
"booting, homing or waiting for its settings); running the test anyway";
|
||
|
||
oneshot_started_ = true;
|
||
handleTest("test " + opts.run_test);
|
||
if (!tester) {
|
||
LOG_ERROR << "--test: '" << opts.run_test << "' did not start (see above)";
|
||
exit_code_ = kExitTestNotRun;
|
||
running = false;
|
||
}
|
||
}
|
||
|
||
void drainCommands() {
|
||
std::queue<std::string> local;
|
||
{
|
||
std::lock_guard<std::mutex> lock(cmd_mutex);
|
||
std::swap(local, cmd_queue);
|
||
}
|
||
while (!local.empty()) {
|
||
handleCommand(local.front());
|
||
local.pop();
|
||
}
|
||
}
|
||
|
||
int run() {
|
||
start_time = std::chrono::steady_clock::now();
|
||
|
||
// Log level: config first, CLI overrides.
|
||
if (!cfg.logging.level.empty() && !Logger::setLevelFromString(cfg.logging.level))
|
||
LOG_WARN << "unknown Logging.level '" << cfg.logging.level << "', keeping default";
|
||
if (!opts.log_level.empty() && !Logger::setLevelFromString(opts.log_level))
|
||
LOG_WARN << "unknown log level '" << opts.log_level << "', keeping default";
|
||
|
||
// Wire-trace categories: config first, CLI overrides (replaces, not merges).
|
||
if (!cfg.logging.trace.empty()) {
|
||
bool ok = true;
|
||
unsigned m = Logger::parseCategories(cfg.logging.trace, &ok);
|
||
if (!ok) LOG_WARN << "unknown trace category in Logging.trace '" << cfg.logging.trace << "'";
|
||
Logger::setCategories(m);
|
||
}
|
||
if (!opts.trace_categories.empty()) {
|
||
bool ok = true;
|
||
unsigned m = Logger::parseCategories(opts.trace_categories, &ok);
|
||
if (!ok) LOG_WARN << "unknown trace category in '" << opts.trace_categories << "'";
|
||
Logger::setCategories(m);
|
||
}
|
||
if (Logger::categories())
|
||
LOG_INFO << "wire trace enabled: " << traceNames(Logger::categories());
|
||
|
||
// Before makeCamera(): the profile is applied at start(). Skipped entirely
|
||
// for a mock or disabled camera - a settings XML configures a physical
|
||
// sensor and nothing else, so requiring one would block bringing a tower
|
||
// up in phases (docs/tower-installation.md section 10).
|
||
if (realCameraWanted()) {
|
||
loadProfiles();
|
||
} else {
|
||
LOG_INFO << "camera profiles not loaded: no real camera in this run";
|
||
}
|
||
|
||
channel = makeChannel();
|
||
motor = makeMotor();
|
||
camera = makeCamera();
|
||
imu = makeImu();
|
||
env = makeEnv();
|
||
|
||
if (!channel->connect())
|
||
LOG_WARN << "Control channel not connected; continuing in degraded mode";
|
||
|
||
// Verify the capture output dir is writable up front; if not (e.g. a
|
||
// misconfigured absolute path), fall back to the per-user default so
|
||
// captures land on disk instead of failing once per frame. Resolved once
|
||
// here, then used everywhere via cfg.paths.output_dir.
|
||
if (!outputDirWritable(cfg.paths.output_dir)) {
|
||
const std::string fb = paths::defaultOutputDir();
|
||
LOG_ERROR << "capture output_dir '" << cfg.paths.output_dir
|
||
<< "' is not writable; falling back to '" << fb << "'";
|
||
cfg.paths.output_dir = fb;
|
||
if (!outputDirWritable(cfg.paths.output_dir))
|
||
LOG_ERROR << "fallback output_dir '" << fb
|
||
<< "' is also not writable; captures will fail to save";
|
||
}
|
||
LOG_INFO << "capture output dir: " << cfg.paths.output_dir;
|
||
|
||
ImagePipeline::Params pp;
|
||
pp.output_dir = cfg.paths.output_dir;
|
||
pp.tower = cfg.general.tower_name;
|
||
pp.demo = opts.demo;
|
||
// In open order, matching Frame::cam_id: the source opens the active
|
||
// cameras in this order too.
|
||
for (const auto* c : cfg.camera.active())
|
||
pp.cameras.push_back({c->label, c->rotation_deg, c->jxl_distance, c->jxl_effort});
|
||
// Orientation supplier, called by ImagePipeline::submit() as each frame
|
||
// arrives. The two axes come from different sources on purpose:
|
||
//
|
||
// yaw - the encoder, as a true compass bearing in [0,360). IMU yaw
|
||
// drifts and depends on the heading reset and the XKF profile,
|
||
// whereas encoder yaw is anchored to the homed centre and tied to
|
||
// north by `gimbal heading setup`, and then stays put.
|
||
// pitch - the IMU. The encoder only measures the axis relative to the
|
||
// gimbal's own base, so a leaning mast is invisible to it; the
|
||
// IMU measures against gravity, which is the true elevation the
|
||
// imagery needs to be georeferenced against.
|
||
pipeline = std::make_unique<ImagePipeline>(
|
||
*channel,
|
||
[this] {
|
||
MotorTelemetry t = motor->telemetry();
|
||
Orientation o{static_cast<float>(cfg.geometry.yaw.headingDeg(t.yaw.xenc)),
|
||
static_cast<float>(cfg.geometry.pitch.toDeg(t.pitch.xenc))};
|
||
if (!imu) return o; // IMU disabled in config: encoder pitch, as before
|
||
if (auto s = imu->sample()) {
|
||
o.pitch_deg = s->pitch_deg;
|
||
imu_pitch_warned_ = false;
|
||
} else if (!imu_pitch_warned_) {
|
||
// Falling back silently would put two different measurements -
|
||
// true elevation and an axis angle - into one archive field
|
||
// with nothing recording which is which. Say it once per outage.
|
||
imu_pitch_warned_ = true;
|
||
LOG_WARN << "IMU has no fresh sample; publishing encoder pitch instead "
|
||
"(CamEvent 'pit' is the axis angle, not true elevation)";
|
||
}
|
||
return o;
|
||
},
|
||
pp);
|
||
|
||
ImagePipeline* pipe_ptr = pipeline.get();
|
||
camera->setFrameCallback([pipe_ptr](const Frame& f) { pipe_ptr->submit(f); });
|
||
|
||
try {
|
||
grid = ScanGrid::fromConfig(cfg.scan);
|
||
} catch (const std::exception& e) {
|
||
scan_load_error_ = e.what();
|
||
LOG_WARN << "scan grid load failed (" << e.what() << "); auto-sweep disabled";
|
||
}
|
||
LOG_INFO << "scan grid: " << grid.size() << " waypoints";
|
||
scheduler = std::make_unique<CaptureScheduler>(*motor, *camera, *channel, cfg.image_rate(),
|
||
cfg.geometry, grid);
|
||
// Every camera has to have finished by the time the gimbal moves on, so
|
||
// the wait is the longest across them, not the first one's.
|
||
scheduler->setSettleWait(cfg.camera.maxSettleWaitMs(), cfg.camera.maxFrameTimeoutMs());
|
||
|
||
// Before the firmware is talked to: the registers are wanted by the very
|
||
// first sendAnchorConfig(), on both the --init and the no---init path.
|
||
planMotorCurrents();
|
||
motor->start();
|
||
if (imu) imu->start();
|
||
if (env) env->start();
|
||
camera->open();
|
||
|
||
// Everything is up, so "found" now means what it says. A tower that
|
||
// declares hardware it does not have stops here rather than quietly
|
||
// capturing a narrower panorama and reporting itself healthy.
|
||
if (!reconcileHardware()) {
|
||
camera->close();
|
||
if (imu) imu->stop();
|
||
if (env) env->stop();
|
||
motor->stop();
|
||
channel->disconnect();
|
||
return 3; // distinct from the config (1) and lock (2) exits
|
||
}
|
||
|
||
pipeline->start();
|
||
channel->publishStatus(0);
|
||
|
||
// Seed an initial snapshot so the UI's first frame is populated, then
|
||
// start the interface (headless console or TUI) on its own thread. The
|
||
// UI is a pure observer: it pulls snapshots and pushes command strings
|
||
// into the same queue the console always used.
|
||
//
|
||
// The UI is brought up BEFORE the init/home sequence so the dashboard is
|
||
// visible during homing (which can take many seconds) and so the serial
|
||
// wire traces render in the TUI's log pane instead of scrolling raw —
|
||
// makeUi()/start() installs the TUI log sink, so anything logged after
|
||
// this point is captured by it.
|
||
publishSnapshot();
|
||
ui = makeUi();
|
||
ui->start([this] { std::lock_guard<std::mutex> lk(snapshot_mutex); return latest_snapshot; },
|
||
[this](const std::string& line) {
|
||
std::lock_guard<std::mutex> lk(cmd_mutex);
|
||
cmd_queue.push(line);
|
||
});
|
||
|
||
if (opts.init) {
|
||
runInitSequence(); // homes, then derives the travel window
|
||
} else {
|
||
// No homing this launch. The firmware keeps nothing across a power
|
||
// cycle, so this only finds limits when the gimbal was already homed
|
||
// earlier in this session (e.g. the host restarted, the board did not).
|
||
// Self-guards to a warning when there is no dump to read.
|
||
sendAnchorConfig();
|
||
applyHomedTravel();
|
||
}
|
||
if (opts.start) startCapture();
|
||
|
||
// Clean up on Ctrl-C / kill / systemd stop, not just on "exit"/Ctrl-D - so the
|
||
// camera is always stopped and closed (an unclean exit wedges it for next time).
|
||
std::signal(SIGINT, handleShutdownSignal);
|
||
std::signal(SIGTERM, handleShutdownSignal);
|
||
|
||
LOG_INFO << "Entering control loop (type 'exit' to quit, or Ctrl-C)";
|
||
oneshot_since_ = std::chrono::steady_clock::now();
|
||
while (running && !g_shutdown_requested.load()) {
|
||
drainCommands();
|
||
driveOneShotTest();
|
||
answerFirmwareNeeds(); // the firmware asks; hardware.ini answers
|
||
pollBackgroundResults(); // apply calibration result / emit DIAG summary+log
|
||
scheduler->tick();
|
||
checkTrackingError(); // warn on live encoder-error excursions
|
||
publishSnapshot();
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||
}
|
||
|
||
LOG_INFO << "Shutting down";
|
||
if (tester) tester->cancel(); // stop any in-flight self-test before teardown
|
||
if (calib) calib->cancel(); // stop any in-flight calibration before teardown
|
||
if (ui) ui->stop();
|
||
pipeline->stop();
|
||
camera->stop();
|
||
camera->close();
|
||
if (imu) imu->stop();
|
||
if (env) env->stop();
|
||
motor->stop();
|
||
channel->disconnect();
|
||
// A --test run interrupted before its verdict has no verdict to report.
|
||
if (!opts.run_test.empty() && running && exit_code_ == 0) exit_code_ = kExitTestNotRun;
|
||
return exit_code_;
|
||
}
|
||
};
|
||
|
||
Application::Application(AppConfig config, RuntimeOptions options)
|
||
: impl_(std::make_unique<Impl>(std::move(config), std::move(options))) {}
|
||
|
||
Application::~Application() = default;
|
||
|
||
int Application::run() { return impl_->run(); }
|
||
|
||
} // namespace fgc
|