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