aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Follow: factor out a Log_Write_FOLL method

Browse Source
This commit is contained in:
Peter Barker 2024-06-06 12:15:15 +10:00 committed by Peter Barker
parent 6765838a3c
commit 89eade0836
2 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Follow/AP_Follow.cpp
View File

@ -411,6 +411,15 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
if (updated) { if (updated) {
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED
Log_Write_FOLL();
#endif
}
}
// write out an onboard-log message to help diagnose follow problems:
#if HAL_LOGGING_ENABLED
void AP_Follow::Log_Write_FOLL()
{
// get estimated location and velocity // get estimated location and velocity
Location loc_estimate{}; Location loc_estimate{};
Vector3f vel_estimate; Vector3f vel_estimate;
@ -445,9 +454,8 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
loc_estimate.lng, loc_estimate.lng,
loc_estimate.alt loc_estimate.alt
); );
#endif
}
} }
#endif // HAL_LOGGING_ENABLED
// get velocity estimate in m/s in NED frame using dt since last update // get velocity estimate in m/s in NED frame using dt since last update
bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const

3
libraries/AP_Follow/AP_Follow.h
View File

@ -136,6 +136,9 @@ private:
// set recorded distance and bearing to target to zero // set recorded distance and bearing to target to zero
void clear_dist_and_bearing_to_target(); void clear_dist_and_bearing_to_target();
// write out an onboard-log message to help diagnose follow problems:
void Log_Write_FOLL();
// parameters // parameters
AP_Int8 _enabled; // 1 if this subsystem is enabled AP_Int8 _enabled; // 1 if this subsystem is enabled
AP_Int16 _sysid; // target's mavlink system id (0 to use first sysid seen) AP_Int16 _sysid; // target's mavlink system id (0 to use first sysid seen)