Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Follow: fixed handling of FOLL_SYSID parameter 

removed separate variable and use parameter only
This commit is contained in:
Andrew Tridgell 2018-12-07 11:05:06 +11:00 committed by Randy Mackay
parent 0440106c16
commit 3d2634671f
2 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Follow/AP_Follow.cpp
View File

@ -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

3
libraries/AP_Follow/AP_Follow.h
View File

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