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

AC_WPNav: add getter for crosstrack_error

This commit is contained in:
Pierre Kancir 2018-05-23 17:34:03 +02:00 committed by Randy Mackay
parent d977ca091a
commit bffe3f6c21
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -356,7 +356,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
track_error = curr_delta - track_covered_pos;
// calculate the horizontal error
float track_error_xy = norm(track_error.x, track_error.y);
_track_error_xy = norm(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
@ -369,7 +369,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
// track_error is the line from the vehicle to the closest point on the track. It is the "opposite" side
// track_leash_slack is the line from the closest point on the track to the target point. It is the "adjacent" side. We adjust this so the track_desired_max is no longer than the leash
float track_leash_length_abs = fabsf(_track_leash_length);
float track_error_max_abs = MAX(_track_leash_length*track_error_z/leash_z, _track_leash_length*track_error_xy/_pos_control.get_leash_xy());
float track_error_max_abs = MAX(_track_leash_length*track_error_z/leash_z, _track_leash_length*_track_error_xy/_pos_control.get_leash_xy());
track_leash_slack = (track_leash_length_abs > track_error_max_abs) ? safe_sqrt(sq(_track_leash_length) - sq(track_error_max_abs)) : 0;
track_desired_max = track_covered + track_leash_slack;
@ -865,7 +865,7 @@ bool AC_WPNav::advance_spline_target_along_track(float dt)
track_error.z -= terr_offset;
// calculate the horizontal error
float track_error_xy = norm(track_error.x, track_error.y);
_track_error_xy = norm(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
@ -880,7 +880,7 @@ bool AC_WPNav::advance_spline_target_along_track(float dt)
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);
float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-_track_error_xy)/leash_xy);
if (track_leash_slack < 0.0f) {
track_leash_slack = 0.0f;
}

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -211,6 +211,9 @@ public:
/// advance_wp_target_along_track - move target location along track from origin to destination
bool advance_wp_target_along_track(float dt);
/// return the crosstrack_error - horizontal error of the actual position vs the desired position
float crosstrack_error() const { return _track_error_xy;}
static const struct AP_Param::GroupInfo var_info[];
protected:
@ -283,6 +286,7 @@ protected:
Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin
Vector3f _destination; // target destination in cm from ekf origin
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
float _track_error_xy; // horizontal error of the actual position vs the desired position
float _track_length; // distance in cm between origin and destination
float _track_length_xy; // horizontal distance in cm between origin and destination
float _track_desired; // our desired distance along the track in cm