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

Plane: base yaw in overshoot on target speed not scaled speed

Browse Source 
this prevents an oscillating due to the speed changing as we yaw,
resulting in a different yaw target
This commit is contained in:
Andrew Tridgell 2022-07-31 15:51:47 +10:00
parent 1d34ac4e05
commit f9f0f60815
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/quadplane.cpp
View File

@ -2559,7 +2559,7 @@ void QuadPlane::vtol_position_controller(void)
// allow up to the WP speed when we are further away, slowing to the pos2 target speed
// when we are close
target_speed = linear_interpolate(position2_target_speed, scaled_wp_speed,
target_speed = linear_interpolate(position2_target_speed, wp_speed,
distance,
position2_dist_threshold*1.5,
2*position2_dist_threshold + stopping_distance(rel_groundspeed_sq));
@ -2567,11 +2567,15 @@ void QuadPlane::vtol_position_controller(void)
target_speed_xy_cms = diff_wp_norm * target_speed * 100;
have_target_yaw = true;
// adjust target yaw angle for wind
// adjust target yaw angle for wind. We calculate yaw based on the target speed
// we want assuming no speed scaling due to direction
const Vector2f wind = plane.ahrs.wind_estimate().xy();
const float gnd_speed = plane.ahrs.groundspeed();
Vector2f target_speed_xy = landing_velocity + diff_wp_norm * gnd_speed - wind;
target_yaw_deg = degrees(target_speed_xy.angle());
// now limit speed to scaled wp speed, so we don't fly rapidly backwards
target_speed_xy.limit_length(scaled_wp_speed*100);
}
}
const float target_speed_ms = target_speed_xy_cms.length() * 0.01;