Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: update crosstrack error during pivot turns

Browse Source
This commit is contained in:
Randy Mackay 2020-08-01 10:50:24 +09:00
parent da746d6e8c
commit 551f7d66c1
2 changed files with 29 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -361,7 +361,7 @@ void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
{
// calculate desired turn rate and update desired heading
if (_pivot_active) {
_cross_track_error = 0.0f;
_cross_track_error = calc_crosstrack_error(current_loc);
_desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;;
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(_desired_heading_cd * 0.01f), radians(_pivot_rate));
@ -458,3 +458,28 @@ void AR_WPNav::apply_speed_min(float &desired_speed)
desired_speed = is_negative(desired_speed) ? -speed_min : speed_min;
}
}
// calculate the crosstrack error (does not rely on L1 controller)
float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const
{
if (!_orig_and_dest_valid) {
return 0.0f;
}
// calculate the NE position of destination relative to origin
Vector2f dest_from_origin = _oa_origin.get_distance_NE(_oa_destination);
// return distance to origin if length of track is very small
if (dest_from_origin.length() < 1.0e-6f) {
return current_loc.get_distance_NE(_oa_destination).length();
}
// convert to a vector indicating direction only
dest_from_origin.normalize();
// calculate the NE position of the vehicle relative to origin
const Vector2f veh_from_origin = _oa_origin.get_distance_NE(current_loc);
// calculate distance to target track, for reporting
return veh_from_origin % dest_from_origin;
}

3
libraries/AR_WPNav/AR_WPNav.h
View File

@ -112,6 +112,9 @@ private:
// adjust speed to ensure it does not fall below value held in SPEED_MIN
void apply_speed_min(float &desired_speed);
// calculate the crosstrack error (does not rely on L1 controller)
float calc_crosstrack_error(const Location& current_loc) const;
private:
// parameters