From 3d2634671f006c97c3b17203511248dd596ca811 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 7 Dec 2018 11:05:06 +1100
Subject: [PATCH] AP_Follow: fixed handling of FOLL_SYSID parameter

removed separate variable and use parameter only
---
 libraries/AP_Follow/AP_Follow.cpp | 14 +++++++-------
 libraries/AP_Follow/AP_Follow.h   |  3 ++-
 2 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp
index b3cbda1602..66134963a2 100644
--- a/libraries/AP_Follow/AP_Follow.cpp
+++ b/libraries/AP_Follow/AP_Follow.cpp
@@ -127,7 +127,6 @@ AP_Follow::AP_Follow() :
         _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     AP_Param::setup_object_defaults(this, var_info);
-    _sysid_to_follow = _sysid;
 }
 
 // get target's estimated location
@@ -238,11 +237,11 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
     }
 
     // skip message if not from our target
-    if ((_sysid_to_follow != 0) && (msg.sysid != _sysid_to_follow)) {
-        if (_sysid == 0) {
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        if (_automatic_sysid) {
             // maybe timeout who we were following...
             if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
-                _sysid_to_follow = 0;
+                _sysid.set(0);
             }
         }
         return;
@@ -290,13 +289,14 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
             _last_heading_update_ms = now;
         }
         // initialise _sysid if zero to sender's id
-        if (_sysid_to_follow == 0) {
-            _sysid_to_follow = msg.sysid;
+        if (_sysid == 0) {
+            _sysid.set(msg.sysid);
+            _automatic_sysid = true;
         }
         if ((now - _last_location_sent_to_gcs) > AP_GCS_INTERVAL_MS) {
             _last_location_sent_to_gcs = now;
             gcs().send_text(MAV_SEVERITY_INFO, "Foll: %u %ld %ld %4.2f\n",
-                            (unsigned)_sysid_to_follow,
+                            (unsigned)_sysid.get(),
                             (long)_target_location.lat,
                             (long)_target_location.lng,
                             (double)(_target_location.alt * 0.01f));    // cm to m
diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h
index 0e27a98b9a..145fce44f3 100644
--- a/libraries/AP_Follow/AP_Follow.h
+++ b/libraries/AP_Follow/AP_Follow.h
@@ -102,7 +102,6 @@ private:
 
     // local variables
     bool _healthy;                  // true if we are receiving mavlink messages (regardless of whether they have target position info within them)
-    uint8_t _sysid_to_follow = 0;   // mavlink system id of vehicle to follow
     uint32_t _last_location_update_ms;  // system time of last position update
     Location _target_location;      // last known location of target
     Vector3f _target_velocity_ned;  // last known velocity of target in NED frame in m/s
@@ -110,4 +109,6 @@ private:
     uint32_t _last_heading_update_ms;   // system time of last heading update
     float _target_heading;          // heading in degrees
     uint32_t _last_location_sent_to_gcs; // last time GCS was told position
+
+    bool _automatic_sysid;          // did we lock onto a sysid automatically?
 };