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

AP_Follow: correct timestamps on GLOBAL_POSITION_INT 

this allows follow to cope better with timing jitter in the telemetry
link between vehicles
This commit is contained in:
Andrew Tridgell 2018-12-30 20:45:32 +11:00 committed by Randy Mackay
parent f1d32df783
commit 42881ecf17
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Follow/AP_Follow.cpp
View File

@ -262,8 +262,6 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
// decode global-position-int message
if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
const uint32_t now = AP_HAL::millis();
// get estimated location and velocity (for logging)
Location loc_estimate{};
Vector3f vel_estimate;
@ -295,10 +293,12 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
_target_velocity_ned.x = packet.vx * 0.01f; // velocity north
_target_velocity_ned.y = packet.vy * 0.01f; // velocity east
_target_velocity_ned.z = packet.vz * 0.01f; // velocity down
_last_location_update_ms = now;
// get a local timestamp with correction for transport jitter
_last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
if (packet.hdg <= 36000) { // heading (UINT16_MAX if unknown)
_target_heading = packet.hdg * 0.01f; // convert centi-degrees to degrees
_last_heading_update_ms = now;
_last_heading_update_ms = _last_location_update_ms;
}
// initialise _sysid if zero to sender's id
if (_sysid == 0) {

4
libraries/AP_Follow/AP_Follow.h
View File

@ -21,6 +21,7 @@
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
#include <AC_PID/AC_P.h>
#include <AP_RTC/JitterCorrection.h>
class AP_Follow
{
@ -124,4 +125,7 @@ private:
bool _automatic_sysid; // did we lock onto a sysid automatically?
float _dist_to_target; // latest distance to target in meters (for reporting purposes)
float _bearing_to_target; // latest bearing to target in degrees (for reporting purposes)
// setup jitter correction with max transport lag of 3s
JitterCorrection _jitter{3000};
};