From d3514f7d2b47b7ef79eb6368f249b80f567e3b27 Mon Sep 17 00:00:00 2001 From: pgdalmeida Date: Wed, 2 Sep 2026 11:50:32 +0200 Subject: [PATCH] Expanded IMU commands; help text shortened --- src/core/Application.cpp | 149 +++++++++++++++++++++++++++++++++++++++ src/core/HelpText.cpp | 97 +++++++++++++++---------- tests/test_helptext.cpp | 18 +++++ 3 files changed, 225 insertions(+), 39 deletions(-) diff --git a/src/core/Application.cpp b/src/core/Application.cpp index 57cac4c..fa8fa56 100644 --- a/src/core/Application.cpp +++ b/src/core/Application.cpp @@ -341,6 +341,153 @@ struct Application::Impl { 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(p.type)) + ")"; + if (cfg_opt->has_scenario && p.type == cfg_opt->scenario_type) out += " *"; + } + return out; + } + + // `imu profile []`. The MTi's XKF profile decides whether heading + // uses the magnetometer, so switching it is how you move between a compass- + // referenced heading and the no-mag profile `gimbal calib` leaves behind. + void imuProfile(const std::string& arg) { + if (arg.empty()) { + LOG_INFO << "XKF profiles (* = active): " << imuProfileList(); + LOG_INFO << "select one with: imu profile "; + 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(c->scenario_type) << ')'; + LOG_INFO << " available (* = active): " << imuProfileList(); + } + + // `imu` command family, mirroring handleGimbal: the operator-facing surface of + // IImuSource. `imu norotation` + `imu zero` are the quick heading calibration + // that gimbal calib runs internally, without the multi-minute sweep. + void handleImu(const std::string& line) { + std::istringstream iss(line); + std::vector tok; + std::string t; + while (iss >> t) tok.push_back(t); // tok[0] == "imu" + if (tok.size() < 2) { + LOG_WARN << "usage: imu "; + return; + } + if (!imu) { + LOG_WARN << "no IMU (needs [Features] enable_imu, or mock_imu)"; + return; + } + + std::string sub = tok[1]; + for (char& ch : sub) ch = static_cast(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 []"; + 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 "; + } + } + std::unique_ptr makeUi() { bool want = opts.use_tui.value_or(cfg.ui.enable_tui); #if FGC_WITH_TUI @@ -1345,6 +1492,8 @@ struct Application::Impl { cancelActiveProcedure(); } else if (c.verb == "gimbal") { handleGimbal(line); + } else if (c.verb == "imu") { + handleImu(line); } else if (c.verb == "calib") { // Answer to the "save calibration?" prompt (activity-strip y/n keys). if (c.device == "save") saveCalibration(); diff --git a/src/core/HelpText.cpp b/src/core/HelpText.cpp index 532668a..1d08d26 100644 --- a/src/core/HelpText.cpp +++ b/src/core/HelpText.cpp @@ -27,16 +27,16 @@ const std::vector& helpCatalog() { {"Positioning", "Aim the gimbal. 'move' is degrees; 'steps' is raw encoder counts.", { {"gimbal move ,", "Point the gimbal at an absolute heading/elevation in DEGREES.", { - "Converts degrees to encoder counts (operator-calibrated) and sends a", - "two-axis move so both axes start together; soft-clamped to travel limits.", + "Converts to encoder counts with the operator calibration and moves both", + "axes together, soft-clamped to the travel limits.", "Example: gimbal move 30,-10 (yaw 30 deg, pitch -10 deg)"}}, {"gimbal steps ,", "Move both axes to absolute encoder COUNTS (no degree conversion).", { "Example: gimbal steps 100000,250000"}}, {"gimbal nudge <+/-pct>", "Relative step move by a percent of the axis travel.", { - "Arrow keys do this when NO overlay is open: Left/Right = yaw -/+5%,", - "Up/Down = pitch +/-10%. Inside an overlay the arrows scroll instead.", + "The arrow keys do this when no overlay is open; inside an overlay they", + "scroll instead.", "Example: gimbal nudge yaw -5"}}, {"gimbal home [y|p]", "Run the endstop-finding home sequence (both axes, or one).", { @@ -48,44 +48,65 @@ const std::vector& helpCatalog() { {"gimbal reset [y|p] / gimbal enable|disable / gimbal setpos ", "Other firmware motor controls.", {}}, }}, + {"IMU", "Read and configure the Xsens MTi orientation sensor.", { + {"imu status", + "Print the latest orientation reading.", { + "Yaw/pitch/roll, device temperature and sample counter. The Sensors panel", + "and the 'i' view show the same data continuously in the TUI."}}, + {"imu config", + "Re-read the device configuration and print it.", { + "Identity, firmware, sample rate, active XKF profile and the available", + "list. Re-reads first, so it never reports a stale profile."}}, + {"imu profile []", + "Select the XKF filter profile (persists on the device).", { + "The profile decides whether heading uses the magnetometer: 'gimbal calib'", + "leaves the device on a no-mag one, where yaw is drift-corrected but not", + "compass-referenced. Typical MTi profiles are General, High_mag_dep,", + "Dynamic and VRU_general; run with no argument for this device's real list.", + "Example: imu profile General / imu profile 39"}}, + {"imu zero", + "Redefine the current pose as yaw 0 (heading reset).", { + "Run 'imu norotation' first so the gyro bias is corrected. Mid-scan this", + "re-references every later CamEvent heading."}}, + {"imu norotation []", + "Run the gyro-bias update; hold the unit still (default 3 s).", { + "Cuts heading drift, which matters most on a no-mag profile. The command", + "returns immediately - keeping the unit still is up to you.", + "Example: imu norotation 5"}}, + }}, {"Diagnostics", "Inspect, self-test and calibrate the gimbal.", { {"gimbal dump", "Request a full firmware state dump and show it here.", { - "Captured DUMP BEGIN..END block (build, uptime, reset cause, per-axis", - "TMC5160 registers); shown in the gimbal 'g' expanded view.", - "Equivalent offline tool: ./firmware/dump.sh"}}, + "Build, uptime, reset cause and the per-axis TMC5160 registers; also shown", + "in the gimbal 'g' view. Offline equivalent: ./firmware/dump.sh"}}, {"test [ []] []", - "Run the hardware self-test suite; a text report is saved to logs/test_*.log.", { - "Modular: 'test' runs everything; 'test gimbal' / 'test imu' / 'test host'", - "narrow it; 'test gimbal homing' runs one leaf. A trailing token names a", - "config profile (repetition counts + thresholds). Each run is compared to a", - "pinned baseline and the previous run (drift flagged). Gimbal/IMU leaves need", - "a homed gimbal / the IMU; 'test host' (thermal/disk/memory) needs neither.", - "Esc cancels a running test. 'test baseline' pins the last report; 'test list'", - "shows profiles."}}, + "Run the hardware self-test suite; a report lands in logs/test_*.log.", { + "'test' runs everything; a subsystem or leaf narrows it, and a trailing", + "token names a config profile. Each run is compared to a pinned baseline", + "and the previous run. Esc cancels.", + "Example: test gimbal homing / test list / test baseline"}}, {"gimbal calib", "Calibrate steps<->degrees using the IMU (auto-homes; ~minutes).", { - "Needs the IMU. Homes first if needed, calibrates PITCH at the first yaw", - "position, moves pitch to 0, switches the IMU to a no-mag XKF profile", - "(persists on the device), runs no-rotation + heading reset to zero yaw", - "drift, then calibrates YAW. Applies the fit to the session and writes", - "logs/calib_*.log. 'gimbal stop' cancels."}}, + "Fits pitch then yaw, applies the result to the session and writes", + "logs/calib_*.log. Leaves the IMU on a no-mag XKF profile. 'gimbal stop'", + "cancels."}}, {"gimbal status", "Ask the firmware to emit one telemetry (ST) line now.", {}}, {"gimbal raw \"\"", - "Low-level: send the quoted text to the firmware verbatim (a newline is", { - "added automatically). Bypasses all wrappers/unit conversion for direct", - "firmware control — use with care; sends exactly what you type.", + "Send the quoted text to the firmware verbatim.", { + "Bypasses all unit conversion and wrappers - use with care; sends exactly", + "what you type, plus a newline.", "Example: gimbal raw \"MOVE 100000,250000\" / gimbal raw \"DUMP\""}}, }}, {"Capture", "Control image capture and encoding.", { {"start", "Begin the capture scan.", {}}, {"stop", "Halt the capture scan.", {}}, {"set fps ", "Set capture rate in images/second.", {}}, - {"set camera mode ", "Switch the camera mode (acquisition rate, exposure " - "limits, imaging settings).", { - "Modes are files in [Camera] mode_dir - one .ini per mode; the name is the", - "filename stem. Example: set camera mode night"}}, + {"set camera mode ", + "Switch the camera mode (rate, exposure limits, imaging settings).", { + "Modes are files in [Camera] mode_dir, one .ini each; the name is the", + "filename stem.", + "Example: set camera mode night"}}, {"set camera jxlq ", "Set JPEG-XL distance (lower = higher quality).", {}}, {"set camera jxle ", "Set JPEG-XL encode effort.", {}}, {"set camera display <0|1>", "Toggle the local display window.", {}}, @@ -99,32 +120,30 @@ const std::vector& helpCatalog() { {"Display", "Terminal dashboard keys (TUI only).", { {"? / g / i / c", "Toggle the Help, Gimbal, Sensors and Camera overlays.", { - "An overlay takes over the body, so it gets the full terminal height.", - "Esc closes it; with no overlay open, Esc cancels a running procedure."}}, + "An overlay takes the full terminal height. Esc closes it; with none open,", + "Esc cancels a running procedure."}}, {"Up/Down, j/k", "Scroll the open overlay one line (Help: change section).", { "The scrollbar on the right shows how much is off-screen."}}, {"PageUp/PageDown", "Scroll the open overlay a screenful; on the dashboard, the log.", { - "Paging the log freezes it: new lines keep arriving but the view", - "holds still. The LOG title shows 'scrollback' while it is frozen."}}, + "Paging the log freezes it: new lines keep arriving but the view holds", + "still, and the LOG title shows 'scrollback'."}}, {"Home/End", "Jump to the top/bottom; on the log, End resumes following the tail.", {}}, {"p", "Freeze the 10 Hz repaint so terminal text selection survives.", { - "Any keypress still repaints, so scrolling while paused drops a", - "selection - pause is for copying, not for reading scrollback."}}, + "Any keypress repaints, so scrolling while paused drops the selection -", + "pause is for copying, not for reading scrollback."}}, }}, {"Session", "Help and exit.", { {"refresh", "Re-read live device state (TUI: press 'r').", { - "Re-queries the IMU configuration (XKF profile, output settings) and", - "requests a fresh firmware dump, updating the 'i' and 'g' expanded views.", - "Use after changing the XKF profile externally. Briefly pauses the IMU", - "stream while it re-enters the device's Config state."}}, + "Re-queries the IMU configuration and requests a firmware dump, updating", + "the 'i' and 'g' views. Briefly pauses the IMU stream."}}, {"help [topic]", "Show this reference; 'help ' expands one section.", { - "Example: help positioning / help dump"}}, + "Example: help positioning / help imu"}}, {"exit", "Shut down and quit.", {}}, }}, }; @@ -137,7 +156,7 @@ std::vector renderHelp(const std::string& topic) { const std::string q = lower(topic); if (q.empty()) { - out.emplace_back("Available commands (type 'help ' for detail, e.g. 'help goto'):"); + out.emplace_back("Available commands (type 'help ' for detail, e.g. 'help imu'):"); for (const auto& sec : helpCatalog()) { out.emplace_back(""); out.emplace_back("== " + sec.title + " == " + sec.blurb); diff --git a/tests/test_helptext.cpp b/tests/test_helptext.cpp index 8bd3c4c..57da3c8 100644 --- a/tests/test_helptext.cpp +++ b/tests/test_helptext.cpp @@ -56,6 +56,24 @@ TEST_CASE("renderHelp() matches the gimbal commands, case-insensitively") CHECK(lower == upper); } +TEST_CASE("renderHelp(imu) matches the whole imu family") { + // `imu` is both a section title and the verb of every entry in it, so the + // topic lookup must find the family either way. + std::string out = join(renderHelp("imu")); + for (const char* sub : {"imu status", "imu config", "imu profile", "imu zero", + "imu norotation"}) + CHECK(out.find(sub) != std::string::npos); + CHECK(join(renderHelp("imu")) == join(renderHelp("IMU"))); +} + +TEST_CASE("help detail stays a short paragraph") { + // The catalog is operator-facing reference, not prose: cap it so entries do + // not creep back to half-screen blocks. + for (const auto& sec : helpCatalog()) + for (const auto& e : sec.entries) + CHECK(e.detail.size() <= 5); +} + TEST_CASE("renderHelp(unknown) reports no match") { std::string out = join(renderHelp("definitely-not-a-command")); CHECK(out.find("No help topic") != std::string::npos);