From 8eda0b4a3698cb7f56914908ced21fe41bab13af Mon Sep 17 00:00:00 2001
From: pgdalmeida <pedro.almeida@ggs-solutions.eu>
Date: Wed, 9 Sep 2026 13:04:25 +0200
Subject: [PATCH] Added IMU pitch to CamEvent; Changed IMU reading to before
 jxl encoding to avoid lag

---
 docs/architecture.md         |  6 ++++--
 docs/modules-reference.md    |  2 +-
 docs/mqtt-api.md             | 28 ++++++++++++++++++++++++++--
 include/fgc/ImagePipeline.h  | 21 +++++++++++++++++++--
 src/camera/ImagePipeline.cpp | 19 +++++++++++++------
 src/core/Application.cpp     | 32 +++++++++++++++++++++++++++++---
 6 files changed, 92 insertions(+), 16 deletions(-)

diff --git a/docs/architecture.md b/docs/architecture.md
index 02a7deb..6f78780 100644
--- a/docs/architecture.md
+++ b/docs/architecture.md
@@ -80,8 +80,10 @@ corrupted.
    auto-exposure tracking the light; `trigger()` hands over the freshest complete frame. See *Image
    capture* below.
 6. **Frame handling** — the kept `Frame` goes to the callback, which `submit()`s it to the
-   `ImagePipeline`. The worker rotates it by `Camera.rotation_deg`, encodes JPEG XL (or copies the demo image), writes
-   `<output_dir>/<label>/<unix_ms>.jxl`, and publishes a `CamEvent` (yaw + pitch from the encoders).
+   `ImagePipeline`. `submit()` samples the gimbal pose there and then — yaw from the encoder, pitch from the IMU —
+   and the pose travels through the queue with the frame, so encode latency cannot attribute a later pose to earlier
+   pixels. The worker rotates it by `Camera.rotation_deg`, encodes JPEG XL (or copies the demo image), writes
+   `<output_dir>/<label>/<unix_ms>.jxl`, and publishes a `CamEvent` carrying that pose.
 
 ## Capture state machine
 
diff --git a/docs/modules-reference.md b/docs/modules-reference.md
index 80bb7eb..934f2ac 100644
--- a/docs/modules-reference.md
+++ b/docs/modules-reference.md
@@ -56,7 +56,7 @@ Per-file reference for the refactored tree, plus the shared data structures.
 | [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp) | `IUserInterface` over FTXUI: panels, key bar, log pane | `WITH_TUI` |
 | [src/camera/VimbaCameraSource.cpp](../src/camera/VimbaCameraSource.cpp) | `ICameraSource` over Vimba X (pImpl): free-running stream + keep-latest, restart-on-stall, and per-camera profiles applied with the SDK's `LoadSettings` on the Viewer's settings XML | `WITH_VIMBA` |
 | [src/camera/JpegXlEncoder.cpp](../src/camera/JpegXlEncoder.cpp) | libjxl encode-to-file | always |
-| [src/camera/ImagePipeline.cpp](../src/camera/ImagePipeline.cpp) | frame → rotate → encode → write → CamEvent (worker thread) | always |
+| [src/camera/ImagePipeline.cpp](../src/camera/ImagePipeline.cpp) | frame → rotate → encode → write → CamEvent (worker thread). The pose is sampled in `submit()` and carried with the frame, so it belongs to the pixels rather than to whenever the encode finished | always |
 
 ## Mock / null implementations
 
diff --git a/docs/mqtt-api.md b/docs/mqtt-api.md
index 8101b28..f68b584 100644
--- a/docs/mqtt-api.md
+++ b/docs/mqtt-api.md
@@ -70,13 +70,37 @@ Built by `MqttControlChannel::publishCamEvent` from a `CamEvent`:
 |-------|------|---------|
 | `fwt` | string | Tower name (`config.ini` `tower_name`) |
 | `cam` | string | Camera label: `RGB`, `ACR`, or `NIR` (by camera index) |
-| `hdg` | int | Gimbal yaw heading **× 10** (one decimal place encoded as integer) at capture time |
-| `pit` | int | Gimbal pitch elevation **× 10** at capture time (derived from the pitch encoder) |
+| `hdg` | int | Gimbal yaw heading **× 10** (one decimal place encoded as integer) at capture time, from the **yaw encoder** |
+| `pit` | int | Gimbal pitch elevation **× 10** at capture time, from the **IMU** — elevation relative to gravity, i.e. true pointing angle |
 | `time` | int | Capture timestamp, Unix epoch **milliseconds** (matches the `.jxl` filename) |
 
 > The `time` value is the same Unix-ms timestamp used as the image filename, so a consumer can locate the file
 > for a given event: `<RGB|ACR|NIR>/<time>.jxl`.
 
+**`pit` is measured against gravity, `hdg` against the encoder.** The two axes come from
+different sources deliberately. The pitch 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
+and therefore reports the elevation the imagery is actually pointing at. Yaw stays on the
+encoder because IMU yaw drifts and depends on the heading reset and XKF profile, whereas
+encoder yaw is calibrated against the IMU by `gimbal calib` and then holds.
+
+Two consequences for consumers:
+
+- **`pit` no longer agrees with the commanded waypoint by construction.** Commanding `+10.0`
+  and receiving `pit: 104` is correct — the residual is real tilt, not error. Before this
+  change both came off the same encoder scale and agreed trivially.
+- **Archives spanning the change contain two different measurements** in this field: an axis
+  angle before it, a true elevation after. If the mast is level the two agree; if it leans,
+  they differ by the lean.
+
+If the IMU is disabled (`[Features] enable_imu = false`), `pit` falls back to the encoder as
+before. If the IMU is enabled but has no fresh sample, it also falls back — and logs a
+warning saying so, once per outage, since that mixes the two measurements in one field.
+
+Both `hdg` and `pit` are sampled **when the frame arrives**, not when its JPEG XL encode
+finishes, so a slow encode or a backed-up queue cannot attribute a later pose to earlier
+pixels.
+
 ### Env payload
 
 Built by `MqttControlChannel::publishEnv` from an `EnvEvent`, sourced from whichever `IEnvSensor` backend is
diff --git a/include/fgc/ImagePipeline.h b/include/fgc/ImagePipeline.h
index 80806b9..366611b 100644
--- a/include/fgc/ImagePipeline.h
+++ b/include/fgc/ImagePipeline.h
@@ -48,6 +48,8 @@ public:
     };
 
     // orientation: returns the current gimbal yaw+pitch (degrees) for CamEvent.
+    // Called from submit(), i.e. on the camera's delivery thread at the moment the
+    // frame arrives - see submit() for why it must not be sampled any later.
     ImagePipeline(IControlChannel& channel, std::function<Orientation()> orientation, Params params);
     ~ImagePipeline();
 
@@ -55,6 +57,14 @@ public:
     void stop();
 
     // Enqueue a frame for processing (thread-safe; copies the frame).
+    //
+    // The gimbal orientation is sampled HERE, not in the worker, and travels with
+    // the frame. Sampling it after encoding would attribute to the image whatever
+    // direction the gimbal had reached by the time the encode finished - which is
+    // only harmless while captures happen at standstill and nothing overlaps. The
+    // moment encoding runs while the gimbal is already slewing to the next
+    // waypoint, or the queue backs up, the published heading and pitch belong to a
+    // different pose than the pixels.
     void submit(const Frame& frame);
 
     // Runtime-adjustable encoder settings (console "set camera ..." commands).
@@ -72,8 +82,15 @@ public:
     long long imagesSaved() const { return saved_.load(); }
 
 private:
+    // A frame and the pose it was taken at, captured together at submit() time so
+    // encoding latency and queue depth cannot separate them.
+    struct QueuedFrame {
+        Frame       frame;
+        Orientation orientation;
+    };
+
     void run();
-    void process(const Frame& frame);
+    void process(const QueuedFrame& queued);
     // Output settings for a camera index, or sane defaults if the index is out of
     // range (a source delivering more cameras than were configured).
     const CameraOutput& outputFor(int cam_id) const;
@@ -82,7 +99,7 @@ private:
     std::function<Orientation()> orientation_;
     Params                       params_;
 
-    std::queue<Frame>       queue_;
+    std::queue<QueuedFrame> queue_;
     std::mutex              mutex_;
     std::condition_variable cv_;
     std::atomic<bool>       running_{false};
diff --git a/src/camera/ImagePipeline.cpp b/src/camera/ImagePipeline.cpp
index 3b04780..186c32c 100644
--- a/src/camera/ImagePipeline.cpp
+++ b/src/camera/ImagePipeline.cpp
@@ -44,24 +44,28 @@ void ImagePipeline::stop() {
 }
 
 void ImagePipeline::submit(const Frame& frame) {
+    // Sample the pose now, while the frame is fresh, and carry it through the
+    // queue. Both sources behind this callback are mutex-guarded snapshot reads,
+    // so it costs the camera's delivery thread a lock and a copy.
+    QueuedFrame q{frame, orientation_ ? orientation_() : Orientation{}};
     {
         std::lock_guard<std::mutex> lock(mutex_);
-        queue_.push(frame);
+        queue_.push(std::move(q));
     }
     cv_.notify_one();
 }
 
 void ImagePipeline::run() {
     while (running_) {
-        Frame frame;
+        QueuedFrame queued;
         {
             std::unique_lock<std::mutex> lock(mutex_);
             cv_.wait(lock, [this] { return !queue_.empty() || !running_; });
             if (!running_ && queue_.empty()) break;
-            frame = std::move(queue_.front());
+            queued = std::move(queue_.front());
             queue_.pop();
         }
-        process(frame);
+        process(queued);
     }
 }
 
@@ -74,7 +78,8 @@ const ImagePipeline::CameraOutput& ImagePipeline::outputFor(int cam_id) const {
     return fallback;
 }
 
-void ImagePipeline::process(const Frame& frame) {
+void ImagePipeline::process(const QueuedFrame& queued) {
+    const Frame& frame = queued.frame;
     if (frame.channels != 1 && frame.channels != 3) {
         LOG_WARN << "Dropping frame with unsupported channel count " << frame.channels;
         return;
@@ -122,7 +127,9 @@ void ImagePipeline::process(const Frame& frame) {
         }
     }
 
-    Orientation o = orientation_ ? orientation_() : Orientation{};
+    // Sampled at submit(), not here: by this point the encode above has run and
+    // the gimbal may already be slewing to the next waypoint.
+    const Orientation& o = queued.orientation;
     CamEvent ev;
     ev.tower           = params_.tower;
     ev.camera          = label;
diff --git a/src/core/Application.cpp b/src/core/Application.cpp
index 56d8820..96d5b71 100644
--- a/src/core/Application.cpp
+++ b/src/core/Application.cpp
@@ -231,6 +231,10 @@ struct Application::Impl {
     mutable ImuConfigView    imu_config_view_;     // formatted MTi config (read once)
     mutable bool             imu_config_done_ = false;
     mutable long long        last_env_published_ts_ = 0;  // gate MQTT publish to one per fresh sample
+    // 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};
 
     std::atomic<bool>       running{true};
     std::mutex              cmd_mutex;
@@ -1746,13 +1750,35 @@ struct Application::Impl {
         // 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: convert the per-axis encoder counts to degrees.
+        // Orientation supplier, called by ImagePipeline::submit() as each frame
+        // arrives. The two axes come from different sources on purpose:
+        //
+        //   yaw   - the encoder. IMU yaw drifts and depends on the heading reset
+        //           and the XKF profile, whereas encoder yaw is calibrated against
+        //           the IMU by `gimbal calib` 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();
-                return Orientation{static_cast<float>(cfg.geometry.yaw.toDeg(t.yaw.xenc)),
-                                   static_cast<float>(cfg.geometry.pitch.toDeg(t.pitch.xenc))};
+                Orientation    o{static_cast<float>(cfg.geometry.yaw.toDeg(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);