Improve ROI LOS rates and align camera telemetry timing

Makefile

@@ -170,6 +170,8 @@ sitl-test: sitl
 sitl-mavlink-test: sitl
 	python3 sitl/test_mavlink_parameters.py --backend mt11 --build $(SITL_BUILD)
 	python3 sitl/test_roi_motion.py --build $(SITL_BUILD)
+	python3 sitl/test_roi_motion.py --build $(SITL_BUILD) --jitter
+	python3 sitl/test_roi_motion.py --build $(SITL_BUILD) --jitter --rate
 	$(SITL_VIDEO_PYTHON) sitl/test_gimbal_angle_hold.py --build $(SITL_BUILD)
 
 a8_sitl-mavlink-test: a8_sitl

camera_app/.gitignore

@@ -10,6 +10,7 @@ tests/test_mp4_rollover
 tests/test_protocol
 tests/test_mavlink
 tests/test_mavlink_time
+tests/test_telemetry_time
 tests/test_gimbal_rate
 tests/test_e5739
 tests/test_thermal

camera_app/Makefile

@@ -275,6 +275,11 @@ tests/test_mavlink_time: tests/test_mavlink_time.cpp src/protocol/mavlink_server
 	$(HOST_CXX) $(CPPFLAGS) $(APP_CXXFLAGS) -ffunction-sections -fdata-sections \
 		-Wl,--gc-sections -o $@ $< src/protocol/mavlink.cpp -lm
 
+tests/test_telemetry_time: tests/test_telemetry_time.cpp src/protocol/mavlink_server.cpp \
+                          include/camera_app/telemetry_time.h src/protocol/targeting.cpp src/protocol/mavlink.cpp $(MAVLINK_DEPS)
+	$(HOST_CXX) $(CPPFLAGS) $(APP_CXXFLAGS) -ffunction-sections -fdata-sections \
+		-Wl,--gc-sections -o $@ $< src/protocol/targeting.cpp src/protocol/mavlink.cpp -lm
+
 $(CONFIG_TEST_TARGET): tests/test_config.cpp src/config.cpp include/camera_app/config.h $(APCAM_HEADERS)
 	$(HOST_CXX) $(CPPFLAGS) $(APP_CXXFLAGS) -o $@ $(filter %.cpp,$^)
 
@@ -527,7 +532,7 @@ $(LIVE_VIDEO_SERVER_TEST_TARGET): tests/test_live_video_server.cpp \
 		src/streaming/live_video_server.cpp src/recording/mp4_minimp4.cpp \
 		src/log.cpp src/recording/video_metadata.cpp src/recording/metadata.cpp -lpthread -lm
 
-test: system-stats-test thermal-mavlink-test media-overlay-test z1-overlay-race-test manual-control-test tests/test_gimbal_rate tests/test_mavlink_time tests/test_support_mavlink $(VIDEO_METADATA_TEST_TARGET) $(TEST_TARGET) $(MAVLINK_TEST_TARGET) $(MP4_TEST_TARGET) $(E5739_TEST_TARGET) \
+test: system-stats-test thermal-mavlink-test media-overlay-test z1-overlay-race-test manual-control-test tests/test_gimbal_rate tests/test_mavlink_time tests/test_telemetry_time tests/test_support_mavlink $(VIDEO_METADATA_TEST_TARGET) $(TEST_TARGET) $(MAVLINK_TEST_TARGET) $(MP4_TEST_TARGET) $(E5739_TEST_TARGET) \
 	$(MP4_SYNC_TEST_TARGET) \
 	$(MP4_ROLLOVER_TEST_TARGET) \
 	$(THERMAL_TEST_TARGET) $(RAW_THERMAL_TEST_TARGET) \
@@ -538,6 +543,7 @@ test: system-stats-test thermal-mavlink-test media-overlay-test z1-overlay-race-
 	$(HOST_TARGET) $(A8_HOST_TARGET)
 	./$(TEST_TARGET)
 	./$(MAVLINK_TEST_TARGET)
+	./tests/test_telemetry_time
 	./tests/test_mavlink_time
 	./tests/test_gimbal_rate
 	./tests/test_support_mavlink
@@ -598,7 +604,7 @@ install-init: $(APP_INIT) $(APP_SELECTION) $(TIMESYNC)
 
 clean:
 	rm -f tests/test_gimbal_rate
-	rm -f tests/test_mavlink_time
+	rm -f tests/test_telemetry_time tests/test_mavlink_time
 	rm -f tests/test_support_mavlink tests/test_thermal_monitor $(MP4_SYNC_TEST_TARGET) $(MP4_ROLLOVER_TEST_TARGET)
 	rm -f tests/test_system_stats tests/test_binlog_sys tests/test_manual_control $(TARGET) $(COMMON_SOURCES:.cpp=.o) $(COMMON_SOURCES:.cpp=.d) $(TEST_TARGET) $(MAVLINK_TEST_TARGET) $(MP4_TEST_TARGET) $(E5739_TEST_TARGET) $(THERMAL_TEST_TARGET) $(RAW_THERMAL_TEST_TARGET) $(RAW_THERMAL_SERVER_TEST_TARGET) $(LIVE_VIDEO_SERVER_TEST_TARGET) $(VIDEO_METADATA_TEST_TARGET) $(STILL_TEST_TARGET) $(AUTOFOCUS_TEST_TARGET) $(CONFIG_TEST_TARGET) $(TARGETING_TEST_TARGET) $(MT11_IMX586_TEST_TARGET) $(HOST_TARGET) $(A8_HOST_TARGET)
 	rm -f tests/test.h264 tests/test.mp4 tests/test.mp4.fragmented \

camera_app/README.md

@@ -240,6 +240,41 @@ location capability and test ArduPilot-calculated angle targeting instead.
 After a camera-app restart, its device information is announced again so a
 connected ArduPilot instance sees the updated capability.
 
+Vehicle boot timestamps are mapped into the camera's monotonic clock using the
+minimum-delay estimator from ArduPilot `AP_RTC/JitterCorrection`. Separate
+estimators for `AUTOPILOT_STATE_FOR_GIMBAL_DEVICE`, fallback `ATTITUDE`, and
+`GLOBAL_POSITION_INT` remove variable transport delay and follow clock drift.
+They use a 500 ms maximum estimated lag and 100-sample convergence window.
+Duplicate and reordered timestamps do not refresh state; 32-bit timestamp wraps
+are unwrapped, and a backwards clock after a stream outage is reacquired.
+Legacy streams with zero timestamps use arrival time until a timestamp is known.
+The estimator cannot measure a constant one-way delay or a device's internal
+sampling delay.
+
+For earth-frame gimbal status, the camera interpolates vehicle yaw and yaw rate
+at the gimbal feedback timestamp, with wrapped-angle interpolation. The yaw
+history retains older samples when buffered telemetry shares a corrected
+millisecond, replacing only the newest value. The outgoing
+`GIMBAL_DEVICE_ATTITUDE_STATUS.time_boot_ms` identifies that feedback sample;
+resending cached feedback keeps its timestamp. MT11 feedback has no device clock,
+so its receive timestamp remains the best available sample-time estimate. If
+matching vehicle history is unavailable, status uses the vehicle frame and marks
+it accordingly. Extrapolation is limited to 250 ms; rate tracking stops when
+required vehicle data cannot be predicted to the current time within that limit.
+`ATTITUDE` takes over as soon as the primary attitude exceeds that prediction
+horizon. Near vertical pitch, where its Euler yaw rate is undefined, it still
+updates attitude metadata but cannot supply control prediction. Metadata ages
+also include the estimated transport lag.
+
+Rate-based ROI tracking derives earth-frame pitch and yaw LOS rates directly
+from `GLOBAL_POSITION_INT` position and NED velocity for a stationary geographic
+ROI. It differentiates the great-circle bearing and elevation geometry without
+rounding a future position to integer latitude/longitude. Motor yaw feed-forward
+is the earth-frame LOS rate minus the vehicle yaw rate. The existing pointing
+correction, rate limits, motor quantisation and lag still apply. At an undefined
+bearing (within 10 cm horizontally of the ROI, or at a geographic pole), rate
+tracking stops instead of producing a singular rate.
+
 `uart.protocol` selects `none`, `siyi` or `mavlink` for the external
 flight-controller connection on `/dev/ttyAMA4`. The selected protocol uses
 230400 baud, 8 data bits, no parity and one stop bit. This UART selection is
@@ -275,9 +310,9 @@ by UTF-8 JSON without a trailing NUL. The schema is `apcg.telemetry.v1`:
 | `pts90k` | Frame timestamp in 90 kHz ticks. RTSP uses the 32-bit RTP timestamp (wraps); MP4 uses its video sample timeline starting at zero. |
 | `utc_us` | Camera system UTC time in microseconds since the Unix epoch, when telemetry was sampled. |
 | `position` | `lat_e7`, `lon_e7` in degrees × 10⁷; `alt_amsl_m`, `alt_relative_m` in metres; `age_ms`. |
-| `vehicle_attitude` | `roll_rad`, `pitch_rad`, `yaw_rad`, `age_ms` and optional `yaw_rate_rad_s` (earth-frame Euler yaw rate). Prefers `AUTOPILOT_STATE_FOR_GIMBAL_DEVICE`; falls back to `ATTITUDE` after one second without it. |
+| `vehicle_attitude` | `roll_rad`, `pitch_rad`, `yaw_rad`, `age_ms` and optional `yaw_rate_rad_s` (earth-frame Euler yaw rate). Prefers `AUTOPILOT_STATE_FOR_GIMBAL_DEVICE`; falls back to `ATTITUDE` after the 250 ms prediction horizon expires. |
 | `velocity` | `vn_m_s`, `ve_m_s`, `vd_m_s` (North/East/Down metres per second), and `age_ms`, from `GLOBAL_POSITION_INT` or `AUTOPILOT_STATE_FOR_GIMBAL_DEVICE`. |
-| `gimbal_attitude` | Mounting-corrected `roll_rad`, `pitch_rad`, `yaw_rad` and `age_ms`; yaw is relative to the vehicle. |
+| `gimbal_attitude` | Mounting-corrected `roll_rad`, `pitch_rad`, `yaw_rad`, `age_ms` and optional measured `yaw_rate_rad_s`; yaw and its rate are relative to the vehicle. |
 | `heading_rad` | Vehicle heading from the position message, in radians. |
 | `zoom` | Camera zoom factor, or null if unknown. |
 | `hfov_deg` | Effective horizontal field of view of this video stream in degrees, including optical zoom and digital crop; null if unknown. |

camera_app/include/camera_app/metadata.h

@@ -37,6 +37,7 @@ struct ca_metadata {
     float gimbal_roll_rad;
     float gimbal_pitch_rad;
     float gimbal_yaw_rad;
+    float gimbal_yaw_rate_rad_s;
     /* 0 when unknown */
     float zoom;
     /* Effective horizontal FOV after optical/digital zoom, degrees; 0 unknown.
@@ -45,18 +46,23 @@ struct ca_metadata {
 };
 
 void ca_metadata_set_model(const char *model);
+/* Optional timestamps are sample times in CLOCK_MONOTONIC milliseconds;
+ * zero retains arrival-time behaviour for local/legacy producers. */
 void ca_metadata_set_position(int32_t lat_e7, int32_t lon_e7, float alt_amsl_m,
-                              float alt_relative_m, float heading_rad);
+                              float alt_relative_m, float heading_rad, uint64_t timestamp_ms = 0);
 void ca_metadata_set_vehicle_attitude(float roll_rad, float pitch_rad,
                                       float yaw_rad);
 void ca_metadata_set_gimbal_attitude(float roll_rad, float pitch_rad,
                                      float yaw_rad);
-void ca_metadata_set_velocity(float vn_m_s, float ve_m_s, float vd_m_s);
+void ca_metadata_set_velocity(float vn_m_s, float ve_m_s, float vd_m_s, uint64_t timestamp_ms = 0);
 void ca_metadata_set_vehicle_attitude_motion(float roll_rad, float pitch_rad,
-                                             float yaw_rad, float yaw_rate_rad_s);
+                                             float yaw_rad, float yaw_rate_rad_s, uint64_t timestamp_ms = 0);
 /* Preserve the backend sample time when polling cached gimbal state. */
 void ca_metadata_set_gimbal_attitude_sample(float roll_rad, float pitch_rad,
                                             float yaw_rad, uint64_t timestamp_ms);
+void ca_metadata_set_gimbal_attitude_motion(float roll_rad, float pitch_rad,
+                                            float yaw_rad, float yaw_rate_rad_s,
+                                            uint64_t timestamp_ms);
 void ca_metadata_set_zoom(float zoom);
 /* copies the current state; sources older than CA_METADATA_MAX_AGE_MS are
  * reported as absent */

camera_app/include/camera_app/targeting.h

@@ -17,4 +17,12 @@ bool ca_targeting_global_angles(int32_t vehicle_lat_e7,
                                 float *pitch_rad,
                                 float *yaw_earth_rad);
 
+/* Instantaneous earth-frame LOS rates to a stationary ROI, using vehicle NED
+ * velocity. Returns false at undefined bearings (within 10 cm or at a pole). */
+bool ca_targeting_global_rates(int32_t vehicle_lat_e7, int32_t vehicle_lon_e7,
+                               float vehicle_alt_amsl_m, int32_t target_lat_e7,
+                               int32_t target_lon_e7, float target_alt_amsl_m,
+                               float vn, float ve, float vd,
+                               float *pitch_rate, float *yaw_rate);
+
 #endif

camera_app/include/camera_app/telemetry_time.h

@@ -0,0 +1,118 @@
+/*
+ * Minimum-delay clock mapping adapted from ArduPilot AP_RTC/JitterCorrection.
+ * Copyright ArduPilot contributors. SPDX-License-Identifier: GPL-3.0-or-later
+ */
+#pragma once
+
+#include <math.h>
+#include <stdint.h>
+
+/* Zero-initialise each instance. Use a separate clock per telemetry stream:
+ * their transport delays differ, and some senders use different boot epochs.
+ * This estimates variable delay, not the unknown minimum one-way latency. */
+struct ca_telemetry_clock {
+    int64_t offset_us, minimum_us;
+    uint64_t last_raw, remote_ticks, received_us;
+    unsigned count;
+    bool initialised;
+
+    bool sample(uint64_t raw, uint32_t tick_us, uint64_t now_us,
+                uint64_t &sample_ms, bool &reset)
+    {
+        reset = false;
+        // Zero is also used by legacy senders with no usable timestamp.
+        if (raw == 0 && !initialised) {
+            sample_ms = now_us / 1000;
+            return true;
+        }
+        if (raw > uint64_t(INT64_MAX) / tick_us) return false;
+        int64_t delta = initialised ? int64_t(raw) - int64_t(last_raw) : 0;
+        // Handles 32-bit milliseconds and ArduPilot's uint32 micros() carried
+        // in the nominally 64-bit AUTOPILOT_STATE time_boot_us field.
+        if (initialised && raw <= UINT32_MAX && last_raw <= UINT32_MAX) {
+            if (delta < -INT64_C(0x80000000)) delta += INT64_C(0x100000000);
+            else if (delta > INT64_C(0x80000000)) delta -= INT64_C(0x100000000);
+        }
+        if (initialised && delta <= 0) {
+            // Ignore duplicates/reordering without refreshing sample age.
+            // A backwards clock after a stream outage indicates a restart.
+            if (delta >= -int64_t(1000000 / tick_us) ||
+                now_us - received_us <= 1000000) return false;
+            *this = {};
+            reset = true;
+        }
+        remote_ticks = initialised ? remote_ticks + delta : raw;
+        last_raw = raw;
+        received_us = now_us;
+        const int64_t remote_us = int64_t(remote_ticks * tick_us);
+        const int64_t diff = int64_t(now_us) - remote_us;
+        if (!initialised || diff < offset_us) offset_us = diff;
+        initialised = true;
+        int64_t estimate = remote_us + offset_us;
+        // The same 500 ms maximum lag and 100-sample drift window as AP_RTC.
+        if (estimate + 500000 < int64_t(now_us)) {
+            estimate = int64_t(now_us) - 500000;
+            offset_us = estimate - remote_us;
+        }
+        if (count == 0 || diff < minimum_us) minimum_us = diff;
+        if (++count == 100) {
+            offset_us = minimum_us;
+            count = 0;
+        }
+        sample_ms = uint64_t(estimate > 0 ? estimate : 0) / 1000;
+        return true;
+    }
+};
+
+/* Vehicle yaw in the camera's monotonic clock. Rates are Euler yaw rates.
+ * Keep enough samples to bracket cached gimbal feedback even at 100 Hz. */
+struct ca_yaw_history {
+    static constexpr unsigned prediction_ms = 250;
+    struct sample { uint64_t ms; float yaw, rate; } samples[128];
+    unsigned count, next;
+
+    void add(uint64_t ms, float yaw, float rate)
+    {
+        if (count) {
+            sample &last = samples[(next + 127) % 128];
+            // Buffered samples can share a corrected millisecond. Replace
+            // the newest value without losing older feedback's brackets.
+            // Clock resets are handled explicitly by the caller.
+            if (ms < last.ms) return;
+            if (ms == last.ms) {
+                last = {ms, yaw, rate};
+                return;
+            }
+        }
+        samples[next] = {ms, yaw, rate};
+        next = (next + 1) % 128;
+        if (count < 128) count++;
+    }
+
+    bool at(uint64_t ms, float &yaw, float &rate) const
+    {
+        if (!count) return false;
+        const sample &last = samples[(next + 127) % 128];
+        if (ms >= last.ms) {
+            if (ms - last.ms > prediction_ms) return false;
+            rate = last.rate;
+            yaw = remainderf(last.yaw + rate * ((ms - last.ms) * .001f), 2 * float(M_PI));
+            return true;
+        }
+        const sample *previous = &samples[(next + 128 - count) % 128];
+        if (ms < previous->ms) return false;
+        for (unsigned i = 1; i < count; i++) {
+            const sample &s = samples[(next + 128 - count + i) % 128];
+            if (ms <= s.ms) {
+                if (s.ms - previous->ms > prediction_ms) return false;
+                float fraction = float(ms - previous->ms) / (s.ms - previous->ms);
+                yaw = remainderf(previous->yaw + fraction * remainderf(s.yaw - previous->yaw,
+                                  2 * float(M_PI)), 2 * float(M_PI));
+                rate = previous->rate + fraction * (s.rate - previous->rate);
+                return true;
+            }
+            previous = &s;
+        }
+        return false;
+    }
+};