Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: auto-yaw-correct-xtract uses target velocity instead of position error

Browse Source 
Co-authored-by: Leonard Hall <leonardthall@gmail.com>
This commit is contained in:
Randy Mackay 2021-03-26 16:51:44 +09:00
parent afa59b9a60
commit 224cd5a0d3
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduSub/control_auto.cpp
View File

@ -514,9 +514,12 @@ float Sub::get_auto_heading()
float track_bearing = get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());
// Bearing from current position towards intermediate position target (centidegrees)
float desired_angle = pos_control.get_bearing_to_target();
float angle_error = wrap_180_cd(desired_angle - track_bearing);
const Vector2f target_vel_xy{pos_control.get_vel_target().x, pos_control.get_vel_target().y};
float angle_error = 0.0f;
if (target_vel_xy.length() >= pos_control.get_max_speed_xy() * 0.1f) {
const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f;
angle_error = wrap_180_cd(desired_angle_cd - track_bearing);
}
float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f);
return wrap_360_cd(track_bearing + angle_limited);
}