From 242e0a7c0afdf2bd98377e3db9607b15e5933151 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 14 Jul 2018 15:42:04 +0900 Subject: [PATCH] AP_Follow: fix initialisation of offsets --- libraries/AP_Follow/AP_Follow.cpp | 4 ++-- libraries/AP_Follow/AP_Follow.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 4d5f915a8d..31355f2750 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -329,11 +329,11 @@ bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const return true; } -// initialise offsets to provided distance vector (in meters in NED frame) if required +// initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required void AP_Follow::init_offsets_if_required(const Vector3f &dist_vec_ned) { if (_offset.get().is_zero()) { - _offset = dist_vec_ned; + _offset = -dist_vec_ned; } } diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index bca5480928..8fb3f64c45 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -81,7 +81,7 @@ private: // get velocity estimate in m/s in NED frame using dt since last update bool get_velocity_ned(Vector3f &vel_ned, float dt) const; - // initialise offsets to provided distance vector (in meters in NED frame) if required + // initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required void init_offsets_if_required(const Vector3f &dist_vec_ned); // get offsets in meters in NED frame