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

@@ -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");
     }
 }
 

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};