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