diff --git a/.deploy.env b/.deploy.env index 8c57d17..637bf3c 100644 --- a/.deploy.env +++ b/.deploy.env @@ -2,4 +2,4 @@ REMOTE_HOST=ggs@10.11.12.111 REMOTE_DIR=/home/ggs/projects/fwt_2a/software CMAKE_ARGS="-DWITH_MQTT=ON -DWITH_VIMBA=ON" -RUN_ARGS="--init --start --trace serial" +RUN_ARGS="--start --trace serial" diff --git a/README.md b/README.md index ad31b1e..06dafa6 100644 --- a/README.md +++ b/README.md @@ -101,8 +101,9 @@ While running, the program reads commands from stdin (one per line): | `gimbal home [y\|p]` | Run the endstop-finding home sequence. | | `gimbal dump` | Request a firmware state dump (shown in the gimbal `g` view). | | `gimbal diag [y\|p\|all]` | Run the firmware motor self-test (results in the activity strip + `logs/`). | -| `gimbal calib` | IMU-referenced steps↔degrees calibration (needs homed axes + IMU; applied to the session). | +| `gimbal calib` | IMU-referenced steps↔degrees calibration (auto-homes if needed; needs the IMU; applied to the session). | | `gimbal …` | Other motor controls — `steps`/`nudge`/`stop`/`speed`/`reset`/`enable`/`disable`/`setpos`/`status`. | +| `gimbal raw ""` | **Low-level:** send the quoted text to the firmware verbatim (newline added). Bypasses wrappers/unit conversion — use with care. | | `debug` | Toggle debug-level logging on/off. | | `set fps ` | Capture rate, in images per second. | | `set camera fps ` | Camera sensor frame rate. | diff --git a/deploy.sh b/deploy.sh index 42b97ff..7246f13 100755 --- a/deploy.sh +++ b/deploy.sh @@ -8,6 +8,7 @@ # Usage: # ./deploy.sh rsync + remote configure + build # ./deploy.sh --run ... then run the app over ssh (RUN_ARGS, ctrl-c to stop) +# ./deploy.sh --run --home ... and run the endstop-finding home sequence at startup # ./deploy.sh --clean wipe the remote build dir first (fresh configure) # ./deploy.sh --check-deps only check the remote build dependencies # ./deploy.sh --run --force skip the placeholder-config guard before running @@ -38,16 +39,24 @@ CLEAN=0 RUN=0 CHECK_DEPS=0 FORCE=0 +HOME_INIT=0 for arg in "$@"; do case "$arg" in --clean) CLEAN=1 ;; --run|--start) RUN=1 ;; + --home|--init) HOME_INIT=1 ;; --check-deps) CHECK_DEPS=1 ;; --force) FORCE=1 ;; *) echo "unknown option: $arg" >&2; exit 1 ;; esac done +# Homing is opt-in: append --init only when --home is passed, so a plain run +# brings up the TUI without driving the endstop-finding sequence. +if [[ $HOME_INIT -eq 1 && "$RUN_ARGS" != *--init* ]]; then + RUN_ARGS="--init $RUN_ARGS" +fi + # Locate cmake on the remote. Non-interactive ssh shells don't source .bashrc, # so allow an explicit REMOTE_CMAKE override in .deploy.env; otherwise search # the usual locations. diff --git a/docs/configuration.md b/docs/configuration.md index c71bbef..3750084 100644 --- a/docs/configuration.md +++ b/docs/configuration.md @@ -59,7 +59,39 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config The firmware reports only **encoder counts**; `[Motor]` maps them to the heading/elevation degrees used by MQTT (`target_HDG`) and `CamEvent`. Calibrate `*_counts_per_deg` / `*_zero_count` against -real `xenc` readings after homing (`MOVE` a known angle, read the resulting `xenc`). +real `xenc` readings after homing (`MOVE` a known angle, read the resulting `xenc`), **or** run the +automated IMU-referenced `gimbal calib` (below). + +#### `gimbal calib` — automated IMU-referenced calibration + +`gimbal calib` (requires the IMU) fits each axis's `counts_per_deg` / `zero_count` by sweeping it and +correlating encoder counts with the MTi's measured angle. The sequence is deliberately ordered so the +**yaw fit is not spoiled by the IMU's heading drift** (see the no-magnetometer discussion below): + +1. **Home if needed** — if the gimbal is not already `READY`, it runs the endstop-finding home first. +2. **Pitch at the first yaw position** — yaw moves to the start of its travel and holds there while + pitch is swept across its soft-limit travel; at each step it dwells and records the + gravity-referenced IMU **pitch** (stable, absolute), then least-squares fits pitch. +3. **Pitch → 0°** — using the just-fitted pitch map. +4. **Switch to a no-magnetometer XKF profile** — picked from the device's own available-profiles list + (a `*nomag*` profile, else a `VRU` one), so the IMU's heading stops chasing the stepper-distorted + magnetic field. Sent in the MTi's Config state, so it **persists on the device** across power-cycles. + Best-effort: if the device reports no magnetometer-free profile, this step is skipped (logged) and + calibration continues. +5. **Drift-correct + zero the heading** — holding the gimbal still, it runs the MTi **no-rotation** + gyro-bias update (cuts yaw drift), then a **heading reset** so the current pose becomes yaw 0. This + happens right before the yaw sweep, so any drift accrued during the slow pitch sweep is discarded. +6. **Yaw sweep** — yaw is swept from that first (now zero-heading) position and fitted. + +After calibration the IMU is left on the no-mag profile (this is intentional — the gimbal's homed +encoders are the absolute heading reference; the IMU only needs stable roll/pitch and short-term +yaw-rate). The `i` view's **XKF profile** row will show the new selection (press `r` to refresh if you +changed it outside calibration). + +Each axis fit reports an R² (shown in the gimbal `g` view + activity strip). The result is applied to +the live session immediately; in the TUI the activity strip then offers to save it to `[Motor]` as the +new default (`y`/`n`). `gimbal stop` cancels a run. Timings (dwell, no-rotation duration, etc.) are the +`CalibParams` defaults in [CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h). The capture **scan grid** is the ordered `(yaw,pitch)` waypoints auto-sweep visits (ping-pong). Set `[Scan] grid_file` to an editable CSV ([config/scan.csv](../config/scan.csv)) to define exact @@ -91,6 +123,10 @@ called a "scenario" in the device manual (e.g. `Machine_nomagfield`); it is the the wire MIDs. Older MTi firmware that does not answer the `Req*` queries simply leaves the section absent (the host logs `no configuration acks received`). +The config is read **once at startup** and cached, so if you change the XKF profile externally the view +stays stale until you **refresh** it: press `r` (or type `refresh`). That re-queries the device (briefly +pausing the stream) and also requests a fresh firmware dump for the gimbal `g` view. + ### Secrets `mqtt_user` / `mqtt_pw` are read from the environment variables **`FGC_MQTT_USER` / `FGC_MQTT_PW`** first, @@ -187,8 +223,9 @@ restarts; press **`n`** to keep it for this session only. The keys are active on showing. (Until you answer `y`, calibration remains session-only — see [known-issues.md](known-issues.md).) -Keys (bottom bar): `s` start · `x` stop · `h` home · `r` reset · `g` gimbal view · `i` IMU view · -arrow keys nudge the gimbal (yaw ±5 % / pitch ±10 %) · `:` open a command line (any console/`gimbal …` +Keys (bottom bar): `s` start · `x` stop · `h` home · `g` gimbal view · `i` IMU view · `r` refresh +(re-read IMU config + firmware dump) · arrow keys nudge the gimbal (yaw ±5 % / pitch ±10 %) · `:` open +a command line (any console/`gimbal …` command) · `?` help · `q` quit. Plain letters are used rather than Ctrl chords so terminal flow-control (`Ctrl-S`/`Ctrl-Q` XON/XOFF) can't swallow them. In TUI mode all log output is diverted from stdout into the on-screen log pane via a `Logger` sink, so the screen is never corrupted. diff --git a/docs/modules-reference.md b/docs/modules-reference.md index 6cb6e57..eaf5dab 100644 --- a/docs/modules-reference.md +++ b/docs/modules-reference.md @@ -17,8 +17,8 @@ Per-file reference for the refactored tree, plus the shared data structures. | [include/fgc/DumpParser.h](../include/fgc/DumpParser.h), [src/core/DumpParser.cpp](../src/core/DumpParser.cpp) | `parseDump` firmware `DUMP` block → `DumpData` (per-axis state + decoded TMC registers/flags); `formatDump` | | [include/fgc/DiagParser.h](../include/fgc/DiagParser.h), [src/core/DiagParser.cpp](../src/core/DiagParser.cpp) | `parseDiag` firmware `DG` self-test stream → `DiagResult` (per-axis tests, PASS/FAIL); `formatDiag` | | [include/fgc/Calibration.h](../include/fgc/Calibration.h), [src/core/Calibration.cpp](../src/core/Calibration.cpp) | `linearFit` (least-squares, R²) + `circularMeanDeg` for the IMU-referenced calibration | -| [include/fgc/CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h), [src/core/CalibrationRoutine.cpp](../src/core/CalibrationRoutine.cpp) | `gimbal calib` worker thread: sweeps each axis, dwells reading the IMU, fits degrees↔counts; exposes `progress()`/`report()`/`takeResult()` | -| [include/fgc/MtiProtocol.h](../include/fgc/MtiProtocol.h), [src/core/MtiProtocol.cpp](../src/core/MtiProtocol.cpp) | Xsens MTi binary protocol: `MtiFramer` (checksum framing), config-message builders, `parseMTData` → `ImuSample` (temp/acc/gyr/mag/euler), config-readback query builders + `applyImuConfigAck`/`finalizeImuConfig` → `ImuDeviceConfig` (product/firmware/device-id/output mode+settings/sample rate/**XKF scenario**) | +| [include/fgc/CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h), [src/core/CalibrationRoutine.cpp](../src/core/CalibrationRoutine.cpp) | `gimbal calib` worker thread: home-if-needed → pitch sweep at the first yaw position → pitch 0° → switch IMU to a no-mag XKF profile → IMU no-rotation + heading reset → yaw sweep, fitting degrees↔counts; tunable via `CalibParams`; exposes `progress()`/`report()`/`takeResult()` | +| [include/fgc/MtiProtocol.h](../include/fgc/MtiProtocol.h), [src/core/MtiProtocol.cpp](../src/core/MtiProtocol.cpp) | Xsens MTi binary protocol: `MtiFramer` (checksum framing), config-message builders, `parseMTData` → `ImuSample` (temp/acc/gyr/mag/euler), config-readback query builders + `applyImuConfigAck`/`finalizeImuConfig` → `ImuDeviceConfig` (product/firmware/device-id/output mode+settings/sample rate/**XKF scenario**); orientation-control builders `msgSetNoRotation` (gyro-bias update) + `msgResetOrientation` (heading reset / store) + `msgSetFilterProfile` (select XKF profile) + `pickNoMagProfile` (choose a magnetometer-free profile from the available list) | | [include/fgc/CaptureScheduler.h](../include/fgc/CaptureScheduler.h), [src/core/CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp) | Capture state machine over the interfaces; injectable clock; `setGeometry` adopts a recalibration | | [include/fgc/Application.h](../include/fgc/Application.h), [src/core/Application.cpp](../src/core/Application.cpp) | Factory (real vs mock, headless vs TUI), wiring, control loop, `gimbal …` commands, background-result polling, `buildSnapshot()` | | [include/fgc/ui/UiSnapshot.h](../include/fgc/ui/UiSnapshot.h), [src/ui/UiSnapshot.cpp](../src/ui/UiSnapshot.cpp) | Plain-data view model (incl. `ImuView`, `ActivityView`, `CalibResultView`, `DumpView`) + pure formatting helpers (state label/colour, degrees, time-ago, pending-sensors) | @@ -32,7 +32,7 @@ Per-file reference for the refactored tree, plus the shared data structures. | [include/fgc/IMotorController.h](../include/fgc/IMotorController.h) | `IMotorController` | `MotorTelemetry` | | [include/fgc/IControlChannel.h](../include/fgc/IControlChannel.h) | `IControlChannel` | `ControlCommand`, `CamEvent` | | [include/fgc/ICameraSource.h](../include/fgc/ICameraSource.h) | `ICameraSource` | `Frame` | -| [include/fgc/IImuSource.h](../include/fgc/IImuSource.h) | `IImuSource` | `ImuSample` (from `MtiProtocol.h`) | +| [include/fgc/IImuSource.h](../include/fgc/IImuSource.h) | `IImuSource` (incl. `config()`, `refreshConfig()`, `noRotation()`, `headingReset()`, `setFilterProfile()`) | `ImuSample` (from `MtiProtocol.h`) | | [include/fgc/ui/IUserInterface.h](../include/fgc/ui/IUserInterface.h) | `IUserInterface` | `UiSnapshot` | ## Real implementations (SDK-gated) diff --git a/include/fgc/CalibrationRoutine.h b/include/fgc/CalibrationRoutine.h index 6b8e151..b36b7a6 100644 --- a/include/fgc/CalibrationRoutine.h +++ b/include/fgc/CalibrationRoutine.h @@ -39,16 +39,38 @@ struct CalibReport { std::vector axes; }; +// Tunable timings/sizes for a calibration run. Defaults are the production +// values; tests inject tiny ones so a full run completes in milliseconds. +struct CalibParams { + int positions = 10; // sweep steps per axis + int dwell_ms = 5000; // hold at each position while sampling + int sample_ms = 100; // IMU sampling cadence during a dwell + int settle_timeout_ms = 15000; // max wait for a single MOVE to settle + int home_timeout_ms = 90000; // max wait for HOME to reach READY + int norotation_s = 3; // no-rotation gyro-bias update duration + int reset_settle_ms = 1500; // let the filter apply the heading reset + double inset_frac = 0.05; // keep targets off the hard endstops +}; + // `gimbal calib`: an IMU-referenced steps<->degrees calibration. Runs on its own -// thread (the motor/imu/Logger interfaces are thread-safe), sweeping each axis -// across its homed soft-limit travel in equal step intervals, dwelling at each to -// record the IMU orientation, then least-squares fitting counts vs degrees. The -// resulting Geometry is published via takeResult() for the main thread to apply -// to the live session; the raw samples + fit are written to a logfile. Progress -// is streamed to the LOG pane via LOG_INFO. Cancellable and one-at-a-time. +// thread (the motor/imu/Logger interfaces are thread-safe). Sequence: +// 1. home the gimbal if it is not already READY; +// 2. move yaw to its first sweep position and calibrate PITCH there (sweep its +// soft-limit travel, dwelling to record the gravity-referenced IMU pitch); +// 3. move pitch to 0 deg; +// 4. switch the IMU to a no-magnetometer XKF profile (persists on the device) so +// its heading stops chasing the stepper-distorted magnetic field; +// 5. with the gimbal held still, run the IMU no-rotation update (cuts yaw drift) +// then reset the IMU heading so the current pose is yaw 0 — this removes the +// drift accumulated during the slow pitch sweep right before it matters; +// 6. sweep YAW from that first position and calibrate it. +// Each axis fit is least-squares (counts vs degrees). The resulting Geometry is +// published via takeResult() for the main thread to apply; raw samples + fits are +// written to a logfile. Progress streams to the LOG pane. Cancellable, one-at-a-time. class CalibrationRoutine { public: - CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial); + CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial, + CalibParams params = {}); ~CalibrationRoutine(); // Begin on a worker thread. Logs a reason and returns false if already @@ -71,6 +93,7 @@ private: IMotorController& motor_; IImuSource& imu_; Geometry initial_; + CalibParams params_; std::thread thread_; std::atomic running_{false}; diff --git a/include/fgc/IImuSource.h b/include/fgc/IImuSource.h index 6261351..d5feaf6 100644 --- a/include/fgc/IImuSource.h +++ b/include/fgc/IImuSource.h @@ -26,6 +26,27 @@ public: // sample rate, identity, XKF scenario). nullopt if not yet known or the // backend cannot report it. virtual std::optional config() const { return std::nullopt; } + + // Re-read the device configuration (e.g. after the XKF profile was changed + // externally). May briefly pause the measurement stream. Blocking; after it + // returns config() reflects the device's current settings. + virtual void refreshConfig() {} + + // Run the "no rotation" gyro-bias update for `seconds`: the device must be + // held perfectly still while it estimates and cancels gyro bias, which cuts + // heading drift (important in no-magnetometer use). Non-blocking: it sends the + // command; the caller is responsible for keeping the unit still that long. + virtual void noRotation(int seconds) { (void)seconds; } + + // Redefine the *current* heading as yaw = 0 (bore-sighting / heading reset). + // Subsequent yaw is measured relative to this pose. Non-blocking. + virtual void headingReset() {} + + // Select the XKF profile/scenario by its `type` number (from config()'s + // available_profiles). Applied in the device's Config state, so it persists to + // non-volatile memory. Briefly pauses the stream; blocking. Returns true if the + // device reports the new profile afterwards. Default: not supported. + virtual bool setFilterProfile(int type) { (void)type; return false; } }; } // namespace fgc diff --git a/include/fgc/MtiImuSource.h b/include/fgc/MtiImuSource.h index c048e89..eaee8d9 100644 --- a/include/fgc/MtiImuSource.h +++ b/include/fgc/MtiImuSource.h @@ -21,6 +21,10 @@ public: bool connected() const override; std::optional sample() override; std::optional config() const override; + void refreshConfig() override; + bool setFilterProfile(int type) override; + void noRotation(int seconds) override; + void headingReset() override; private: struct Impl; diff --git a/include/fgc/MtiProtocol.h b/include/fgc/MtiProtocol.h index 5a5a209..40812b1 100644 --- a/include/fgc/MtiProtocol.h +++ b/include/fgc/MtiProtocol.h @@ -48,6 +48,15 @@ inline constexpr uint8_t kMidAvailFilterProf = 0x63; inline constexpr uint8_t kMidReqFilterProfile = 0x64; // SetScenario shares this MID inline constexpr uint8_t kMidReqFilterProfileAck = 0x65; +// Orientation-control commands (valid in Measurement State). +inline constexpr uint8_t kMidSetNoRotation = 0x22; // 2-byte duration (seconds) +inline constexpr uint8_t kMidSetNoRotationAck = 0x23; +inline constexpr uint8_t kMidResetOrientation = 0xA4; // 2-byte CODE (Table 33) +inline constexpr uint8_t kMidResetOrientationAck = 0xA5; +// ResetOrientation CODE values we use. +inline constexpr uint16_t kResetHeading = 0x0001; // current heading becomes yaw 0 +inline constexpr uint16_t kResetStore = 0x0000; // persist current reset (Config state) + // OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04). inline constexpr uint16_t kOutputMode = 0x0007; // OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp @@ -116,6 +125,24 @@ struct ImuDeviceConfig { std::vector available_profiles; }; +// Orientation-control command builders. +// SetNoRotation: run the "no rotation" gyro-bias update for `seconds` (the device +// must be held still during it) to cut heading drift, esp. in no-magnetometer use. +std::vector msgSetNoRotation(uint16_t seconds); +// ResetOrientation: CODE 0x0001 redefines the *current* heading as yaw 0 +// (bore-sighting); CODE 0x0000 (in Config state) stores it across power cycles. +std::vector msgResetOrientation(uint16_t code); + +// SetFilterProfile (classic): select the XKF profile/scenario by type number. +// Valid in Config state; settings changed in Config state persist to the device's +// non-volatile memory automatically. +std::vector msgSetFilterProfile(uint16_t profile); + +// Pick the no-magnetometer XKF profile from the device's reported list: prefers a +// label containing "nomag"/"no_mag", else a "VRU" (gyro-tracked heading) profile. +// Returns the profile `type` number, or -1 if none looks magnetometer-free. +int pickNoMagProfile(const std::vector& profiles); + // Config-readback query builders (empty data field => "request", not "set"). std::vector msgReqProductCode(); std::vector msgReqDID(); diff --git a/include/fgc/mock/MockImuSource.h b/include/fgc/mock/MockImuSource.h index cb38931..92d0b4a 100644 --- a/include/fgc/mock/MockImuSource.h +++ b/include/fgc/mock/MockImuSource.h @@ -44,8 +44,23 @@ public: return s; } + void refreshConfig() override { LOG_INFO << "[mock] IMU config refreshed"; } + void noRotation(int seconds) override { + LOG_INFO << "[mock] IMU no-rotation update for " << seconds << " s"; + } + void headingReset() override { + LOG_INFO << "[mock] IMU heading reset (current direction is now yaw 0)"; + } + bool setFilterProfile(int type) override { + scenario_type_ = type; // observable in the next config() + LOG_INFO << "[mock] IMU XKF profile set to type " << type; + return true; + } + // Synthetic config mirroring the real handshake (Euler + calibrated, 100 Hz), - // so the expanded view's IMU CONFIG section renders without hardware. + // so the expanded view's IMU CONFIG section renders without hardware. The + // available list includes a no-mag profile so the calibration's profile switch + // has something to pick. std::optional config() const override { ImuDeviceConfig c; c.valid = true; @@ -64,18 +79,20 @@ public: c.has_period = true; c.period = 1152; // 100 Hz c.sample_rate_hz = 100.0f; - c.has_scenario = true; - c.scenario_type = 39; - c.scenario_version = 11; c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"}, - {41, 11, "Dynamic"}}; - c.scenario_label = "General"; + {41, 11, "Dynamic"}, {53, 11, "VRU_general"}}; + c.has_scenario = true; + c.scenario_type = static_cast(scenario_type_); + c.scenario_version = 11; + for (const auto& p : c.available_profiles) + if (p.type == c.scenario_type) c.scenario_label = p.label; return c; } private: using clock = std::chrono::steady_clock; clock::time_point start_ = clock::now(); + int scenario_type_ = 39; // current XKF profile (mutable via setFilterProfile) }; } // namespace fgc diff --git a/src/core/Application.cpp b/src/core/Application.cpp index 049e8a4..c1adda1 100644 --- a/src/core/Application.cpp +++ b/src/core/Application.cpp @@ -550,7 +550,7 @@ struct Application::Impl { while (iss >> t) tok.push_back(t); // tok[0] == "gimbal" if (tok.size() < 2) { LOG_WARN << "usage: gimbal "; + "speed|setpos|status|dump|diag|calib|raw>"; return; } std::string sub = tok[1]; @@ -603,6 +603,25 @@ struct Application::Impl { } 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 \"\""; + return; + } + LOG_INFO << "gimbal raw -> " << raw; + motor->sendCommand(raw); } else if (sub == "home" || sub == "stop" || sub == "reset" || sub == "enable" || sub == "disable" || sub == "speed" || sub == "setpos" || sub == "status") { // Passthrough to the (case-insensitive) firmware verb + args. @@ -730,6 +749,16 @@ struct Application::Impl { 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") { diff --git a/src/core/CalibrationRoutine.cpp b/src/core/CalibrationRoutine.cpp index 2a4f65c..49b2643 100644 --- a/src/core/CalibrationRoutine.cpp +++ b/src/core/CalibrationRoutine.cpp @@ -19,17 +19,13 @@ namespace fgc { namespace { using clock = std::chrono::steady_clock; -constexpr int kPositions = 10; -constexpr int kDwellMs = 5000; // hold at each position -constexpr int kSampleMs = 100; // IMU sampling cadence during dwell -constexpr int kSettleTimeoutMs = 15000; // max wait for a move to settle -constexpr double kInsetFrac = 0.05; // keep targets off the hard endstops void msleep(int ms) { std::this_thread::sleep_for(std::chrono::milliseconds(ms)); } } // namespace -CalibrationRoutine::CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial) - : motor_(motor), imu_(imu), initial_(std::move(initial)) {} +CalibrationRoutine::CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial, + CalibParams params) + : motor_(motor), imu_(imu), initial_(std::move(initial)), params_(params) {} CalibrationRoutine::~CalibrationRoutine() { cancel(); @@ -86,9 +82,9 @@ long long nowMs() { namespace { // Wait until the axis is at `target` (standstill + within tol), or timeout/cancel. -bool waitSettle(IMotorController& motor, char axis, long target, long tol, +bool waitSettle(IMotorController& motor, char axis, long target, long tol, int timeout_ms, const std::atomic& cancel) { - const auto deadline = clock::now() + std::chrono::milliseconds(kSettleTimeoutMs); + const auto deadline = clock::now() + std::chrono::milliseconds(timeout_ms); while (clock::now() < deadline) { if (cancel) return false; MotorTelemetry t = motor.telemetry(); @@ -99,16 +95,29 @@ bool waitSettle(IMotorController& motor, char axis, long target, long tol, return false; } -// Dwell `kDwellMs` sampling the IMU; return the (circular for yaw) mean of the +// Wait until both present axes report READY (homing complete), or timeout/cancel. +bool waitHomed(IMotorController& motor, int timeout_ms, const std::atomic& cancel) { + const auto deadline = clock::now() + std::chrono::milliseconds(timeout_ms); + while (clock::now() < deadline) { + if (cancel) return false; + MotorTelemetry t = motor.telemetry(); + if (t.yaw.ready() && (!t.pitch_present || t.pitch.ready())) return true; + msleep(200); + } + return false; +} + +// Dwell `dwell_ms` sampling the IMU; return the (circular for yaw) mean of the // requested Euler angle in degrees. -double dwellAndMeasure(IImuSource& imu, char axis, const std::atomic& cancel) { +double dwellAndMeasure(IImuSource& imu, char axis, int dwell_ms, int sample_ms, + const std::atomic& cancel) { std::vector vals; - const auto end = clock::now() + std::chrono::milliseconds(kDwellMs); + const auto end = clock::now() + std::chrono::milliseconds(dwell_ms); while (clock::now() < end) { if (cancel) break; if (auto s = imu.sample()) vals.push_back(axis == 'Y' ? s->yaw_deg : s->pitch_deg); - msleep(kSampleMs); + msleep(sample_ms); } if (vals.empty()) return 0.0; if (axis == 'Y') { @@ -133,8 +142,9 @@ void CalibrationRoutine::run() { } } done{this}; + const int N = params_.positions; LOG_INFO << "=== gimbal calibration starting ==="; - setProgress('?', 0, kPositions, "starting"); + setProgress('?', 0, N, "starting"); CalibReport report; // 1. Preconditions. @@ -142,11 +152,22 @@ void CalibrationRoutine::run() { LOG_WARN << "calibration aborted: IMU not connected"; return; } + + // 2. Home first if the gimbal is not already READY. MotorTelemetry t0 = motor_.telemetry(); - if (!t0.yaw.ready() || (t0.pitch_present && !t0.pitch.ready())) { - LOG_WARN << "calibration aborted: axes must be homed (READY) first"; - return; + bool homed = t0.yaw.ready() && (!t0.pitch_present || t0.pitch.ready()); + if (!homed) { + LOG_INFO << "calibration: gimbal not homed; homing first..."; + setProgress('?', 0, N, "homing"); + motor_.sendCommand("HOME"); + if (!waitHomed(motor_, params_.home_timeout_ms, cancel_)) { + if (cancel_) { LOG_WARN << "calibration cancelled"; return; } + LOG_WARN << "calibration aborted: homing did not reach READY"; + return; + } + t0 = motor_.telemetry(); } + const bool pitch_present = t0.pitch_present; // Fetch soft limits from a fresh dump. LOG_INFO << "calibration: requesting soft limits (dump)..."; @@ -166,73 +187,143 @@ void CalibrationRoutine::run() { std::ostringstream log; log << "gimbal calibration\n==================\n"; - struct AxisJob { char axis; AxisMap* map; }; - std::vector jobs = {{'Y', &result.yaw}}; - if (t0.pitch_present) jobs.push_back({'P', &result.pitch}); - - for (const auto& job : jobs) { - if (cancel_) { LOG_WARN << "calibration cancelled"; return; } + // Per-axis sweep span [a..b] (inset off the hard endstops) + a settle tolerance. + auto axisSpan = [&](char axis, long& a, long& b, long& tol) -> bool { const DumpAxis* da = nullptr; - for (const auto& ax : dump.axes) if (ax.axis == job.axis) da = &ax; - if (!da) { LOG_WARN << "calibration: no limits for axis " << job.axis << "; skipping"; continue; } - + for (const auto& ax : dump.axes) if (ax.axis == axis) da = &ax; + if (!da) return false; const long lo = da->lim_neg, hi = da->lim_pos; const long travel = std::labs(hi - lo); - const long inset = static_cast(kInsetFrac * travel); - const long a = lo + inset, b = hi - inset; - const long tol = std::max(500, travel / 200); + const long inset = static_cast(params_.inset_frac * travel); + a = lo + inset; + b = hi - inset; + tol = std::max(500, travel / 200); + return true; + }; - LOG_INFO << "calibrating " << (job.axis == 'Y' ? "YAW" : "PITCH") - << " over [" << a << ".." << b << "] counts in " << kPositions << " steps"; - log << "\n[" << job.axis << "] travel=[" << lo << ".." << hi << "] tol=" << tol + // Sweep one axis across [a..b] in N steps, dwelling to measure the IMU angle, + // then least-squares fit counts vs degrees and record the result. Returns + // false on cancel; on a degenerate fit it records a failed axis and returns true. + auto sweepAxis = [&](char axis, AxisMap& map, long a, long b, long tol) -> bool { + LOG_INFO << "calibrating " << (axis == 'Y' ? "YAW" : "PITCH") + << " over [" << a << ".." << b << "] counts in " << N << " steps"; + log << "\n[" << axis << "] sweep=[" << a << ".." << b << "] tol=" << tol << "\n target_counts, imu_deg\n"; std::vector> pts; // (deg, counts) double prev_deg = 0.0; bool have_prev = false; - for (int i = 0; i < kPositions; ++i) { - if (cancel_) { LOG_WARN << "calibration cancelled"; return; } - long target = a + (b - a) * i / (kPositions - 1); - setProgress(job.axis, i + 1, kPositions, "moving"); - motor_.sendCommand(std::string("MOVE ") + job.axis + " " + std::to_string(target)); - if (!waitSettle(motor_, job.axis, target, tol, cancel_)) { - if (cancel_) return; - LOG_WARN << " position " << (i + 1) << "/" << kPositions + for (int i = 0; i < N; ++i) { + if (cancel_) { LOG_WARN << "calibration cancelled"; return false; } + long target = a + (b - a) * i / (N - 1); + setProgress(axis, i + 1, N, "moving"); + motor_.sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target)); + if (!waitSettle(motor_, axis, target, tol, params_.settle_timeout_ms, cancel_)) { + if (cancel_) return false; + LOG_WARN << " position " << (i + 1) << "/" << N << " did not settle near " << target << " (continuing)"; } - setProgress(job.axis, i + 1, kPositions, "dwelling"); - double deg = dwellAndMeasure(imu_, job.axis, cancel_); + setProgress(axis, i + 1, N, "dwelling"); + double deg = dwellAndMeasure(imu_, axis, params_.dwell_ms, params_.sample_ms, cancel_); // Unwrap against the previous sample so a sweep crossing 0/360 stays a // continuous line for the fit (no ±360 jump). if (have_prev) deg = unwrapNear(prev_deg, deg); prev_deg = deg; have_prev = true; pts.emplace_back(deg, static_cast(target)); - LOG_INFO << " " << (i + 1) << "/" << kPositions << " counts=" << target + LOG_INFO << " " << (i + 1) << "/" << N << " counts=" << target << " imu=" << deg << " deg"; log << " " << target << ", " << deg << "\n"; } - setProgress(job.axis, kPositions, kPositions, "fitting"); + setProgress(axis, N, N, "fitting"); LinearFit fit = linearFit(pts); if (!fit.ok) { - LOG_WARN << " fit failed for axis " << job.axis << " (degenerate data)"; + LOG_WARN << " fit failed for axis " << axis << " (degenerate data)"; log << " FIT FAILED\n"; - report.axes.push_back({job.axis, false, 0, 0, 0, static_cast(pts.size())}); - continue; + report.axes.push_back({axis, false, 0, 0, 0, static_cast(pts.size())}); + return true; } - const double old_cpd = job.map->counts_per_deg; - const long old_zc = job.map->zero_count; - job.map->counts_per_deg = fit.slope; - job.map->zero_count = static_cast(std::lround(fit.intercept)); + const double old_cpd = map.counts_per_deg; + const long old_zc = map.zero_count; + map.counts_per_deg = fit.slope; + map.zero_count = static_cast(std::lround(fit.intercept)); LOG_INFO << " fit: counts_per_deg " << old_cpd << " -> " << fit.slope - << ", zero_count " << old_zc << " -> " << job.map->zero_count + << ", zero_count " << old_zc << " -> " << map.zero_count << " (R^2=" << fit.r2 << ")"; - log << " fit: counts_per_deg=" << fit.slope << " zero_count=" << job.map->zero_count + log << " fit: counts_per_deg=" << fit.slope << " zero_count=" << map.zero_count << " R2=" << fit.r2 << " (was cpd=" << old_cpd << " zc=" << old_zc << ")\n"; - report.axes.push_back({job.axis, true, fit.slope, job.map->zero_count, fit.r2, fit.n}); + report.axes.push_back({axis, true, fit.slope, map.zero_count, fit.r2, fit.n}); + return true; + }; + + // Resolve the yaw sweep span up front — we move to its first position before + // the pitch sweep so the whole pitch calibration happens at that fixed yaw. + long ya = 0, yb = 0, ytol = 0; + if (!axisSpan('Y', ya, yb, ytol)) { + LOG_WARN << "calibration aborted: no yaw soft limits in dump"; + return; } + // 3. Move yaw to its first sweep position and hold it there. + LOG_INFO << "calibration: moving yaw to first position (counts=" << ya << ")"; + setProgress('Y', 1, N, "moving"); + motor_.sendCommand(std::string("MOVE Y ") + std::to_string(ya)); + waitSettle(motor_, 'Y', ya, ytol, params_.settle_timeout_ms, cancel_); + if (cancel_) { LOG_WARN << "calibration cancelled"; return; } + + // 4. Calibrate PITCH at this yaw position, then 5. move pitch to 0 deg. + if (pitch_present) { + long pa = 0, pb = 0, ptol = 0; + if (axisSpan('P', pa, pb, ptol)) { + if (!sweepAxis('P', result.pitch, pa, pb, ptol)) { return; } // cancelled + const long pzero = result.pitch.toCounts(0.0); + LOG_INFO << "calibration: moving pitch to 0 deg (counts=" << pzero << ")"; + setProgress('P', N, N, "pitch->0"); + motor_.sendCommand(std::string("MOVE P ") + std::to_string(pzero)); + waitSettle(motor_, 'P', pzero, ptol, params_.settle_timeout_ms, cancel_); + } else { + LOG_WARN << "calibration: no pitch soft limits in dump; skipping pitch"; + } + } + if (cancel_) { LOG_WARN << "calibration cancelled"; return; } + + // 6. Switch the IMU to a no-magnetometer XKF profile so its heading stops + // chasing the (stepper-distorted) magnetic field. Selected from the device's + // own available list; persists on the device. Best-effort — if none is + // available we log and continue with the drift correction below. + if (auto cfg = imu_.config()) { + int nomag = pickNoMagProfile(cfg->available_profiles); + if (nomag >= 0 && (!cfg->has_scenario || cfg->scenario_type != nomag)) { + LOG_INFO << "calibration: switching IMU to no-mag XKF profile (type " << nomag << ")"; + setProgress('Y', 0, N, "no-mag profile"); + if (!imu_.setFilterProfile(nomag)) + LOG_WARN << "calibration: IMU did not confirm the no-mag profile (continuing)"; + } else if (nomag < 0) { + LOG_WARN << "calibration: no no-mag XKF profile available on this device (continuing)"; + } + } + if (cancel_) { LOG_WARN << "calibration cancelled"; return; } + + // 7. Hold still: correct yaw drift (no-rotation gyro-bias update) then reset + // the IMU heading so this pose is yaw 0 — done right before the yaw sweep so + // the drift accumulated during the long pitch sweep doesn't bias the fit. + LOG_INFO << "calibration: correcting yaw drift (no-rotation " << params_.norotation_s + << " s, holding still)..."; + setProgress('Y', 0, N, "yaw drift correction"); + imu_.noRotation(params_.norotation_s); + for (int waited = 0; waited < params_.norotation_s * 1000 + 500 && !cancel_; waited += 100) + msleep(100); + if (cancel_) { LOG_WARN << "calibration cancelled"; return; } + LOG_INFO << "calibration: resetting IMU heading (this pose is now yaw 0)"; + setProgress('Y', 0, N, "yaw heading reset"); + imu_.headingReset(); + msleep(params_.reset_settle_ms); + if (cancel_) { LOG_WARN << "calibration cancelled"; return; } + + // 8. Calibrate YAW from this (now zero-heading) first position. + if (!sweepAxis('Y', result.yaw, ya, yb, ytol)) { return; } // cancelled + if (cancel_) { LOG_WARN << "calibration cancelled"; return; } std::string path = paths::writeLogFile(paths::timestampedLogName("calib"), log.str()); diff --git a/src/core/HelpText.cpp b/src/core/HelpText.cpp index 6d50a59..a4d5f9d 100644 --- a/src/core/HelpText.cpp +++ b/src/core/HelpText.cpp @@ -58,12 +58,19 @@ const std::vector& helpCatalog() { "Each axis is swept at several speeds/directions (DG lines). A PASS/FAIL", "summary is logged and saved to logs/diag_*.log. Axes must be homed first."}}, {"gimbal calib", - "Calibrate steps<->degrees using the IMU (sweeps each axis, ~minutes).", { - "Homes-limits required + IMU enabled. Sweeps yaw then pitch in 10 steps,", - "dwells 5 s recording IMU orientation, fits the conversion, applies it to", - "the session and writes logs/calib_*.log. 'gimbal stop' cancels."}}, + "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."}}, {"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.", + "Example: gimbal raw \"MOVE 100000,250000\" / gimbal raw \"DUMP\""}}, }}, {"Capture", "Control image capture and encoding.", { {"start", "Begin the capture scan.", {}}, @@ -81,6 +88,12 @@ const std::vector& helpCatalog() { "Example: trace serial on / trace off"}}, }}, {"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."}}, {"help [topic]", "Show this reference; 'help ' expands one section.", { "Example: help positioning / help dump"}}, diff --git a/src/core/MtiProtocol.cpp b/src/core/MtiProtocol.cpp index db70d48..3c9f772 100644 --- a/src/core/MtiProtocol.cpp +++ b/src/core/MtiProtocol.cpp @@ -1,7 +1,9 @@ #include "fgc/MtiProtocol.h" +#include #include #include +#include #include namespace fgc { @@ -75,6 +77,41 @@ std::vector msgSetOutputSettings() { return mtiMessage(kMidSetOutputSettings, d); } +std::vector msgSetNoRotation(uint16_t seconds) { + std::vector d; + putBE(d, seconds); + return mtiMessage(kMidSetNoRotation, d); +} + +std::vector msgResetOrientation(uint16_t code) { + std::vector d; + putBE(d, code); + return mtiMessage(kMidResetOrientation, d); +} + +std::vector msgSetFilterProfile(uint16_t profile) { + std::vector d; + putBE(d, profile); + return mtiMessage(kMidReqFilterProfile, d); // same MID; non-empty data => "set" +} + +int pickNoMagProfile(const std::vector& profiles) { + auto lower = [](std::string s) { + for (char& c : s) c = static_cast(std::tolower((unsigned char)c)); + return s; + }; + int vru = -1; + for (const auto& p : profiles) { + const std::string l = lower(p.label); + if (l.find("nomag") != std::string::npos || l.find("no_mag") != std::string::npos || + l.find("no mag") != std::string::npos) + return p.type; // explicit no-magnetometer + if (vru < 0 && l.find("vru") != std::string::npos) // gyro-tracked heading + vru = p.type; + } + return vru; +} + std::vector msgReqProductCode() { return mtiMessage(kMidReqProductCode); } std::vector msgReqDID() { return mtiMessage(kMidReqDID); } std::vector msgReqFWRev() { return mtiMessage(kMidReqFWRev); } diff --git a/src/serial/MtiImuSource.cpp b/src/serial/MtiImuSource.cpp index 7a2ddff..7e824c6 100644 --- a/src/serial/MtiImuSource.cpp +++ b/src/serial/MtiImuSource.cpp @@ -72,7 +72,9 @@ struct MtiImuSource::Impl { // Synchronous, best-effort config handshake (no ack parsing): runs on the // calling thread BEFORE the io_thread starts, so there is no concurrent // access to the serial port. Small delays let the device switch states. - void configure() { + // If set_profile >= 0, also selects that XKF profile while in Config state + // (persists to non-volatile memory). + void configure(int set_profile = -1) { using namespace std::chrono_literals; auto write = [this](const std::vector& m) { boost::system::error_code ec; @@ -80,6 +82,10 @@ struct MtiImuSource::Impl { if (ec) LOG_WARN << "MTi config write failed: " << ec.message(); }; write(msgGoToConfig()); std::this_thread::sleep_for(60ms); + if (set_profile >= 0) { + write(msgSetFilterProfile(static_cast(set_profile))); + std::this_thread::sleep_for(60ms); + } write(msgSetOutputMode()); std::this_thread::sleep_for(60ms); write(msgSetOutputSettings()); std::this_thread::sleep_for(60ms); // Read back the device configuration while still in Config State. @@ -89,6 +95,24 @@ struct MtiImuSource::Impl { ::tcflush(serial.native_handle(), TCIFLUSH); } + // Pause the streaming io_thread, re-run the handshake (optionally setting an + // XKF profile), then resume streaming. Cancels + drains the outstanding read + // first so it can't steal the config-ack bytes during the re-query. Called + // from the control thread (no concurrent serial access). Used by refreshConfig + // and setFilterProfile. + void restartWithConfig(int set_profile) { + io.stop(); + if (io_thread.joinable()) io_thread.join(); + io.restart(); + boost::system::error_code ec; + serial.cancel(ec); + static_cast(io.run()); // run the aborted read handler (no reschedule) + io.restart(); + configure(set_profile); + doRead(); + io_thread = std::thread([this] { io.run(); }); + } + // Query the device configuration in Config State and decode the acks. Runs on // the calling thread before the streaming io_thread starts; it drives the // io_context itself with run_for() and leaves it clean (restarted, all @@ -101,6 +125,10 @@ struct MtiImuSource::Impl { applyImuConfigAck(local, mid, d, n); }); + // Drop any buffered pre-config stream (e.g. MTData from before GoToConfig + // took effect) so the read window below is dominated by the query acks. + ::tcflush(serial.native_handle(), TCIFLUSH); + write(msgReqProductCode()); write(msgReqDID()); write(msgReqFWRev()); @@ -204,4 +232,53 @@ std::optional MtiImuSource::config() const { return impl_->cfg; } +// Re-read the device configuration: briefly pause the streaming io_thread, +// re-run the Config-state handshake (which re-queries product/firmware/output/ +// XKF profile and re-applies our output settings), then resume streaming. Mirrors +// start() minus opening the port. Called from the control thread; sample()/config() +// are called from that same thread when building the snapshot, so there is no +// concurrent serial access during the refresh. +void MtiImuSource::refreshConfig() { + if (!impl_->open) { LOG_WARN << "MTi: refresh ignored (not started)"; return; } + LOG_INFO << "MTi: refreshing configuration (briefly pausing the stream)..."; + impl_->restartWithConfig(-1); // re-query only, no profile change +} + +bool MtiImuSource::setFilterProfile(int type) { + if (!impl_->open) { LOG_WARN << "MTi: setFilterProfile ignored (not started)"; return false; } + if (type < 0) return false; + LOG_INFO << "MTi: setting XKF profile to type " << type << " (persists on device)..."; + impl_->restartWithConfig(type); + std::lock_guard lock(impl_->mutex); + return impl_->have_cfg && impl_->cfg.has_scenario && impl_->cfg.scenario_type == type; +} + +// Send a command while the streaming io_thread is running. The write is posted +// onto the io_context so it runs on the io_thread, never concurrently with the +// in-flight async_read (concurrent ops on one serial_port are unsafe — same rule +// as SerialMotorController). +void MtiImuSource::noRotation(int seconds) { + if (!impl_->open) { LOG_WARN << "MTi: noRotation ignored (not started)"; return; } + if (seconds < 1) seconds = 1; + auto msg = std::make_shared>( + msgSetNoRotation(static_cast(seconds))); + boost::asio::post(impl_->io, [this, msg] { + boost::system::error_code ec; + boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec); + if (ec) LOG_WARN << "MTi noRotation write failed: " << ec.message(); + }); + LOG_INFO << "MTi: no-rotation update for " << seconds << " s (hold still)"; +} + +void MtiImuSource::headingReset() { + if (!impl_->open) { LOG_WARN << "MTi: headingReset ignored (not started)"; return; } + auto msg = std::make_shared>(msgResetOrientation(kResetHeading)); + boost::asio::post(impl_->io, [this, msg] { + boost::system::error_code ec; + boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec); + if (ec) LOG_WARN << "MTi headingReset write failed: " << ec.message(); + }); + LOG_INFO << "MTi: heading reset (current direction is now yaw 0)"; +} + } // namespace fgc diff --git a/src/ui/TuiUi.cpp b/src/ui/TuiUi.cpp index 4e7a966..f816155 100644 --- a/src/ui/TuiUi.cpp +++ b/src/ui/TuiUi.cpp @@ -479,7 +479,7 @@ Element imuDetailPanel(const ImuView& v) { Element status = v.present ? (text(" MTi live ") | color(Color::Green) | bold) : (text(" MTi offline ") | color(Color::Red) | bold); - return window(text(" IMU (i/Esc:close) ") | bold | color(Color::Magenta), + return window(text(" IMU (i/Esc:close r:refresh config) ") | bold | color(Color::Magenta), vbox({hbox({status, filler()}), separator(), vbox(std::move(body)) | flex})); } @@ -563,7 +563,8 @@ void TuiUi::uiLoop() { } else { bottom = hbox({ keyHint("s", "Start"), keyHint("x", "Stop"), keyHint("h", "Home"), - keyHint("g", "Gimbal"), keyHint("i", "IMU"), keyHint("\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93", "Nudge"), + keyHint("g", "Gimbal"), keyHint("i", "IMU"), keyHint("r", "Refresh"), + keyHint("\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93", "Nudge"), keyHint(":", "Cmd"), keyHint("?", "Help"), filler(), keyHint("q", "Quit"), }); } @@ -660,7 +661,7 @@ void TuiUi::uiLoop() { if (c == "s") { if (sink_) sink_("start"); return true; } if (c == "x") { if (sink_) sink_("stop"); return true; } if (c == "h") { if (sink_) sink_("gimbal home"); return true; } - if (c == "r") { if (sink_) sink_("gimbal reset"); return true; } + if (c == "r") { if (sink_) sink_("refresh"); return true; } // IMU config + dump if (c == ":") { command_mode = true; return true; } return false; }); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 1a715c8..0670fbe 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -33,6 +33,7 @@ add_executable(fgc_tests test_helptext.cpp test_diagparser.cpp test_calibration.cpp + test_calibroutine.cpp ) target_link_libraries(fgc_tests PRIVATE fgc_core doctest::doctest) diff --git a/tests/test_calibroutine.cpp b/tests/test_calibroutine.cpp new file mode 100644 index 0000000..c042106 --- /dev/null +++ b/tests/test_calibroutine.cpp @@ -0,0 +1,305 @@ +#include + +#include "fgc/CalibrationRoutine.h" +#include "fgc/Geometry.h" +#include "fgc/IImuSource.h" +#include "fgc/IMotorController.h" + +#include +#include +#include +#include +#include +#include + +using namespace fgc; + +namespace { + +// A two-axis firmware DUMP with soft limits (subset of the real format; enough +// for parseDump to expose lim_neg/lim_pos per axis). +const char* kDump = + "DUMP BEGIN build=test uptime=1000 mcusr=0x01 free_ram=1852\n" + "DUMP Y state=3 hsub=10 enabled=1 lim_neg=-80000 lim_pos=80000 hold_target=0 " + "speed=50000 eeprom_restored=0 has_encoder=1\n" + "DUMP Y TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n" + "DUMP P state=3 hsub=10 enabled=1 lim_neg=0 lim_pos=600000 hold_target=0 " + "speed=150000 eeprom_restored=0 has_encoder=1\n" + "DUMP P TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n" + "DUMP END\n"; + +// Counts-per-degree the fake IMU reports, so a clean sweep recovers this slope. +constexpr double kFakeCpd = 10000.0; + +// Fake motor: records every command in order, and "executes" HOME/MOVE instantly +// (axis jumps to target, standstill + READY) so waitSettle/waitHomed return at +// once. IMU angle is derived from the live encoder position (see FakeImu). +struct FakeImu; +struct FakeMotor : IMotorController { + mutable std::mutex mtx; + MotorTelemetry tel; + std::vector events; // unified, ordered log (motor + imu) + bool pitch_present = true; + bool have_dump = true; + + void start() override {} + void stop() override {} + + void record(const std::string& s) { + std::lock_guard lk(mtx); + events.push_back(s); + } + + void sendCommand(const std::string& c) override { + record(c); + std::lock_guard lk(mtx); + std::istringstream iss(c); + std::string verb; + iss >> verb; + if (verb == "HOME") { + tel.yaw.state = AxisState::Ready; tel.yaw.standstill = true; + tel.pitch.state = AxisState::Ready; tel.pitch.standstill = true; + tel.pitch_present = pitch_present; + } else if (verb == "MOVE") { + std::string axis; + long target = 0; + iss >> axis >> target; + AxisTelemetry& a = (axis == "Y") ? tel.yaw : tel.pitch; + a.xenc = target; + a.standstill = true; + a.state = AxisState::Ready; + } + } + + MotorTelemetry telemetry() override { + std::lock_guard lk(mtx); + return tel; + } + std::string lastDump() override { return have_dump ? kDump : ""; } + std::string lastDiag() override { return ""; } + unsigned diagSeq() const override { return 0; } + bool connected() const override { return true; } +}; + +// Fake IMU: pitch is the live pitch encoder / kFakeCpd; yaw is the live yaw +// encoder offset by the heading-reset zero, / kFakeCpd. Records noRotation / +// headingReset into the shared motor event log so ordering can be asserted. +struct FakeImu : IImuSource { + FakeMotor& motor; + long yaw_zero = 0; + int scenario_type = 39; // starts on a mag-using profile + bool has_profiles = true; // expose an available list (incl. a no-mag one) + explicit FakeImu(FakeMotor& m) : motor(m) {} + + void start() override {} + void stop() override {} + bool connected() const override { return true; } + + std::optional sample() override { + MotorTelemetry t = motor.telemetry(); + ImuSample s; + s.valid = true; + s.pitch_deg = static_cast(t.pitch.xenc / kFakeCpd); + double y = (t.yaw.xenc - yaw_zero) / kFakeCpd; + s.yaw_deg = static_cast(y); + return s; + } + + std::optional config() const override { + ImuDeviceConfig c; + c.valid = true; + c.has_scenario = true; + c.scenario_type = static_cast(scenario_type); + if (has_profiles) + c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"}, + {53, 11, "VRU_general"}}; + return c; + } + + void noRotation(int seconds) override { + motor.record("noRotation:" + std::to_string(seconds)); + } + void headingReset() override { + motor.record("headingReset"); + std::lock_guard lk(motor.mtx); + yaw_zero = motor.tel.yaw.xenc; // current pose becomes yaw 0 + } + bool setFilterProfile(int type) override { + motor.record("setProfile:" + std::to_string(type)); + scenario_type = type; + return true; + } +}; + +Geometry uncalibrated() { + Geometry g; + g.yaw = {1.0, 0, -100000.0, 100000.0}; + g.pitch = {1.0, 0, -100000.0, 100000.0}; + return g; +} + +// Fast timings so a full run completes in well under a second. +CalibParams fastParams() { + CalibParams p; + p.positions = 4; + p.dwell_ms = 4; + p.sample_ms = 1; + p.settle_timeout_ms = 200; + p.home_timeout_ms = 500; + p.norotation_s = 0; + p.reset_settle_ms = 2; + return p; +} + +// Run to completion (or fail the test on timeout). +void runToCompletion(CalibrationRoutine& cal) { + REQUIRE(cal.start()); + auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (cal.running()) { + if (std::chrono::steady_clock::now() > deadline) { FAIL("calibration did not finish"); } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } +} + +int indexOf(const std::vector& v, const std::string& needle) { + for (int i = 0; i < static_cast(v.size()); ++i) + if (v[i].find(needle) != std::string::npos) return i; + return -1; +} +int lastIndexOf(const std::vector& v, const std::string& needle) { + int found = -1; + for (int i = 0; i < static_cast(v.size()); ++i) + if (v[i].find(needle) != std::string::npos) found = i; + return found; +} + +} // namespace + +TEST_CASE("calibration homes first when the gimbal is not READY") { + FakeMotor motor; // starts in Boot (not ready) + motor.tel.pitch_present = true; + FakeImu imu(motor); + + CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams()); + runToCompletion(cal); + + const auto& ev = motor.events; + REQUIRE_FALSE(ev.empty()); + CHECK(ev.front() == "HOME"); // homing is the first action + CHECK(indexOf(ev, "HOME") < indexOf(ev, "MOVE P")); // home before any pitch move +} + +TEST_CASE("calibration does NOT home when already READY") { + FakeMotor motor; + motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true; + motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true; + motor.tel.pitch_present = true; + FakeImu imu(motor); + + CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams()); + runToCompletion(cal); + + CHECK(indexOf(motor.events, "HOME") == -1); // no homing issued +} + +TEST_CASE("calibration runs pitch-at-first-yaw, then drift-correct+reset, then yaw") { + FakeMotor motor; + motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true; + motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true; + motor.tel.pitch_present = true; + FakeImu imu(motor); + + CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams()); + runToCompletion(cal); + + const auto& ev = motor.events; + // Yaw is parked at its first position before the pitch sweep begins. + const int first_yaw_move = indexOf(ev, "MOVE Y"); + const int first_pitch_move = indexOf(ev, "MOVE P"); + REQUIRE(first_yaw_move >= 0); + REQUIRE(first_pitch_move >= 0); + CHECK(first_yaw_move < first_pitch_move); + + // No-mag profile switch, drift correction, then heading reset happen AFTER the + // pitch sweep and BEFORE the yaw sweep reaches its far end. + const int prof = indexOf(ev, "setProfile:53"); // VRU_general from the fake list + const int norot = indexOf(ev, "noRotation"); + const int reset = indexOf(ev, "headingReset"); + REQUIRE(prof >= 0); + REQUIRE(norot >= 0); + REQUIRE(reset >= 0); + CHECK(first_pitch_move < prof); // pitch calibrated before the profile switch + CHECK(prof < norot); // switch to no-mag, then correct drift + CHECK(norot < reset); // correct drift, then reset + CHECK(reset < lastIndexOf(ev, "MOVE Y")); // yaw sweep continues after the reset + + // Pitch is commanded to 0 deg before the heading reset. + CHECK(indexOf(ev, "MOVE P 0") >= 0); + CHECK(indexOf(ev, "MOVE P 0") < reset); +} + +TEST_CASE("calibration skips the profile switch when no no-mag profile exists") { + FakeMotor motor; + motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true; + motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true; + motor.tel.pitch_present = true; + FakeImu imu(motor); + imu.has_profiles = false; // device reports no available profiles + + CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams()); + runToCompletion(cal); + + const auto& ev = motor.events; + CHECK(indexOf(ev, "setProfile") == -1); // nothing to switch to + CHECK(indexOf(ev, "headingReset") >= 0); // still resets + sweeps yaw + CHECK(cal.report().valid); +} + +TEST_CASE("calibration recovers the fake IMU slope for both axes") { + FakeMotor motor; + motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true; + motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true; + motor.tel.pitch_present = true; + FakeImu imu(motor); + + CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams()); + runToCompletion(cal); + + CalibReport rep = cal.report(); + REQUIRE(rep.valid); + CHECK(rep.all_ok); + REQUIRE(rep.axes.size() == 2); + for (const auto& ax : rep.axes) { + CHECK(ax.ok); + CHECK(ax.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001)); + CHECK(ax.r2 == doctest::Approx(1.0).epsilon(0.001)); + } + + // The new geometry is published for the session to apply. + auto geo = cal.takeResult(); + REQUIRE(geo.has_value()); + CHECK(geo->yaw.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001)); + CHECK(geo->pitch.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001)); +} + +TEST_CASE("calibration handles a yaw-only gimbal (no pitch axis)") { + FakeMotor motor; + motor.pitch_present = false; + motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true; + motor.tel.pitch_present = false; + FakeImu imu(motor); + + CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams()); + runToCompletion(cal); + + const auto& ev = motor.events; + CHECK(indexOf(ev, "MOVE P") == -1); // no pitch moves + CHECK(indexOf(ev, "headingReset") >= 0); // still resets + sweeps yaw + CHECK(indexOf(ev, "MOVE Y") >= 0); + + CalibReport rep = cal.report(); + REQUIRE(rep.valid); + REQUIRE(rep.axes.size() == 1); + CHECK(rep.axes[0].axis == 'Y'); + CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001)); +} diff --git a/tests/test_mtiprotocol.cpp b/tests/test_mtiprotocol.cpp index 6aa6df9..605f3d3 100644 --- a/tests/test_mtiprotocol.cpp +++ b/tests/test_mtiprotocol.cpp @@ -144,6 +144,59 @@ TEST_CASE("parseMTData reports yaw as a 0..360 heading") { CHECK(got.yaw_deg == doctest::Approx(181.0f)); } +TEST_CASE("orientation-control command builders are well-formed") { + SUBCASE("SetNoRotation carries the duration big-endian") { + auto m = msgSetNoRotation(3); + CHECK(m[0] == kMtiPreamble); + CHECK(m[1] == kMtiBid); + CHECK(m[2] == kMidSetNoRotation); + CHECK(m[3] == 2); + CHECK(m[4] == 0x00); + CHECK(m[5] == 0x03); + CHECK(frameChecksumOk(m)); + } + SUBCASE("ResetOrientation heading reset = CODE 0x0001") { + auto m = msgResetOrientation(kResetHeading); + CHECK(m[2] == kMidResetOrientation); + CHECK(m[3] == 2); + CHECK(m[4] == 0x00); + CHECK(m[5] == 0x01); + CHECK(frameChecksumOk(m)); + } + SUBCASE("ResetOrientation store = CODE 0x0000") { + auto m = msgResetOrientation(kResetStore); + CHECK(m[4] == 0x00); + CHECK(m[5] == 0x00); + CHECK(frameChecksumOk(m)); + } + SUBCASE("SetFilterProfile carries the profile number (set = non-empty data)") { + auto m = msgSetFilterProfile(53); + CHECK(m[2] == kMidReqFilterProfile); // same MID as Req; data makes it a Set + CHECK(m[3] == 2); + CHECK(m[4] == 0x00); + CHECK(m[5] == 53); + CHECK(frameChecksumOk(m)); + } +} + +TEST_CASE("pickNoMagProfile chooses a magnetometer-free profile") { + SUBCASE("prefers an explicit no-mag label") { + std::vector ps = { + {39, 11, "General"}, {40, 11, "High_mag_dep"}, {7, 1, "Machine_nomagfield"}}; + CHECK(pickNoMagProfile(ps) == 7); + } + SUBCASE("falls back to a VRU profile") { + std::vector ps = { + {39, 11, "General"}, {53, 11, "VRU_general"}, {40, 11, "High_mag_dep"}}; + CHECK(pickNoMagProfile(ps) == 53); + } + SUBCASE("returns -1 when every profile uses the magnetometer") { + std::vector ps = { + {39, 11, "General"}, {40, 11, "High_mag_dep"}, {41, 11, "Dynamic"}}; + CHECK(pickNoMagProfile(ps) == -1); + } +} + TEST_CASE("config-readback query builders are well-formed requests (empty data)") { struct Q { std::vector m; uint8_t mid; }; Q qs[] = {