Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: fix camera glitch when in sysid follow mode with unreliable telemetry

This commit is contained in:
Tim Tuxworth 2024-11-05 19:58:22 +08:00
parent e7bfd400e8
commit dac62d3d17
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -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_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_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_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 // Default init function for every mount
void AP_Mount_Backend::init() 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. // 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_alt_cm(packet.alt*0.1, Location::AltFrame::ABSOLUTE);
_target_sysid_location_set = true; _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; return true;
} }
@ -889,6 +892,10 @@ bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const
if (!_target_sysid) { if (!_target_sysid) {
return false; 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); return get_angle_target_to_location(_target_sysid_location, angle_rad);
} }

3
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -29,6 +29,7 @@
#include <RC_Channel/RC_Channel.h> #include <RC_Channel/RC_Channel.h>
#include <AP_Camera/AP_Camera_shareddefs.h> #include <AP_Camera/AP_Camera_shareddefs.h>
#include "AP_Mount.h" #include "AP_Mount.h"
#include <AP_RTC/JitterCorrection.h>
class AP_Mount_Backend class AP_Mount_Backend
{ {
@ -341,6 +342,8 @@ protected:
uint8_t _target_sysid; // sysid to track uint8_t _target_sysid; // sysid to track
Location _target_sysid_location;// sysid target location Location _target_sysid_location;// sysid target location
bool _target_sysid_location_set;// true if _target_sysid has been set 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 uint32_t _last_warning_ms; // system time of last warning sent to GCS