From da0c1d1b42377fb256e6d30fcdf5955fe55cbcc1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 4 Oct 2019 10:19:33 +0900 Subject: [PATCH] AP_Follow: add clear_offsets_if_required method this restores the offsets to zero if they were initialised from zero when the lead vehicle was first spotted --- libraries/AP_Follow/AP_Follow.cpp | 12 ++++++++++++ libraries/AP_Follow/AP_Follow.h | 8 ++++++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index b0893ffdda..2979d1b196 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -134,6 +134,15 @@ AP_Follow::AP_Follow() : AP_Param::setup_object_defaults(this, var_info); } +// restore offsets to zero if necessary, should be called when vehicle exits follow mode +void AP_Follow::clear_offsets_if_required() +{ + if (_offsets_were_zero) { + _offset = Vector3f(); + } + _offsets_were_zero = false; +} + // get target's estimated location bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const { @@ -346,16 +355,19 @@ void AP_Follow::init_offsets_if_required(const Vector3f &dist_vec_ned) if (!_offset.get().is_zero()) { return; } + _offsets_were_zero = true; float target_heading_deg; if ((_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) && get_target_heading_deg(target_heading_deg)) { // rotate offsets from north facing to vehicle's perspective _offset = rotate_vector(-dist_vec_ned, -target_heading_deg); + gcs().send_text(MAV_SEVERITY_INFO, "Relative follow offset loaded"); } else { // initialise offset in NED frame _offset = -dist_vec_ned; // ensure offset_type used matches frame of offsets saved _offset_type = AP_FOLLOW_OFFSET_TYPE_NED; + gcs().send_text(MAV_SEVERITY_INFO, "N-E-D follow offset loaded"); } } diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 2ee40413eb..fc5bc8eba6 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -44,6 +44,9 @@ public: // set which target to follow void set_target_sysid(uint8_t sysid) { _sysid = sysid; } + // restore offsets to zero if necessary, should be called when vehicle exits follow mode + void clear_offsets_if_required(); + // // position tracking related methods // @@ -122,8 +125,9 @@ private: uint32_t _last_heading_update_ms; // system time of last heading update float _target_heading; // heading in degrees 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) + 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) + bool _offsets_were_zero; // true if offsets were originally zero and then initialised to the offset from lead vehicle // setup jitter correction with max transport lag of 3s JitterCorrection _jitter{3000};