AP_Mount: Scripted follow in Plane

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -426,6 +426,9 @@ 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 = AP_HAL::millis();
+    _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 > 200) {
+        return false;
+    }
     return get_angle_target_to_location(_target_sysid_location, angle_rad);
 }
 

libraries/AP_Mount/AP_Mount_Backend.h

@@ -29,6 +29,7 @@
 #include <RC_Channel/RC_Channel.h>
 #include <AP_Camera/AP_Camera_shareddefs.h>
 #include "AP_Mount.h"
+#include <AP_RTC/JitterCorrection.h>
 
 class AP_Mount_Backend
 {
@@ -341,6 +342,9 @@ 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
+    // setup jitter correction with max transport lag of 3s
+    JitterCorrection _jitter{3000};
 
     uint32_t _last_warning_ms;      // system time of last warning sent to GCS