From dac62d3d17aaaef9004de5402c29ee6b00d29de4 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 5 Nov 2024 19:58:22 +0800 Subject: [PATCH] AP_Mount: fix camera glitch when in sysid follow mode with unreliable telemetry --- libraries/AP_Mount/AP_Mount_Backend.cpp | 7 +++++++ libraries/AP_Mount/AP_Mount_Backend.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9d064633b1916..a11a6001eaef5 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -11,6 +11,7 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_POI_REQUEST_TIMEOUT_MS 30000 // POI calculations continue to be updated for this many seconds after last request #define AP_MOUNT_POI_RESULT_TIMEOUT_MS 3000 // POI calculations valid for 3 seconds #define AP_MOUNT_POI_DIST_M_MAX 10000 // POI calculations limit of 10,000m (10km) +#define AP_MOUNT_SYSID_TIMEOUT_MS 600 // SYSID position estimate timeout after .6 second, which from testing seems "good" // Default init function for every mount void AP_Mount_Backend::init() @@ -426,6 +427,8 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli // global_position_int.alt is *UP*, so is location. _target_sysid_location.set_alt_cm(packet.alt*0.1, Location::AltFrame::ABSOLUTE); _target_sysid_location_set = true; + // keep track of when we last received the update + _target_sysid_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis()); return true; } @@ -889,6 +892,10 @@ bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const if (!_target_sysid) { return false; } + // don't update the angle if we haven't received a recent update from the target + if(AP_HAL::millis() - _target_sysid_update_ms > AP_MOUNT_SYSID_TIMEOUT_MS) { + return false; + } return get_angle_target_to_location(_target_sysid_location, angle_rad); } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 16082d58f450b..93a3104f19e99 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -29,6 +29,7 @@ #include #include #include "AP_Mount.h" +#include class AP_Mount_Backend { @@ -341,6 +342,8 @@ class AP_Mount_Backend uint8_t _target_sysid; // sysid to track Location _target_sysid_location;// sysid target location bool _target_sysid_location_set;// true if _target_sysid has been set + uint32_t _target_sysid_update_ms;// timestamp when the target_sysid_location was last updated + JitterCorrection _jitter{3000}; // setup jitter correction for target sysid updates with max transport lag of 3s uint32_t _last_warning_ms; // system time of last warning sent to GCS