Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Follow: factor out a should_handle_message method

Browse Source
This commit is contained in:
Peter Barker 2024-06-06 12:09:45 +10:00 committed by Peter Barker
parent 72fa6aa410
commit 6765838a3c
2 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_Follow/AP_Follow.cpp
View File

@ -272,27 +272,40 @@ bool AP_Follow::get_target_heading_deg(float &heading) const
return true;
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Follow::handle_msg(const mavlink_message_t &msg)
// returns true if we should extract information from msg
bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
{
// exit immediately if not enabled
if (!_enabled) {
return;
return false;
}
// skip our own messages
if (msg.sysid == mavlink_system.sysid) {
return;
return false;
}
// skip message if not from our target
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.set(0);
}
return false;
}
return true;
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Follow::handle_msg(const mavlink_message_t &msg)
{
// this method should be called from an "update()" method:
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.set(0);
}
}
if (!should_handle_message(msg)) {
return;
}

3
libraries/AP_Follow/AP_Follow.h
View File

@ -118,6 +118,9 @@ public:
private:
static AP_Follow *_singleton;
// returns true if we should extract information from msg
bool should_handle_message(const mavlink_message_t &msg) const;
// get velocity estimate in m/s in NED frame using dt since last update
bool get_velocity_ned(Vector3f &vel_ned, float dt) const;