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

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

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 committed by Randy Mackay
parent 9485a9a62c
commit 5aba7bef29
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

@ -2522,7 +2522,7 @@ void QuadPlane::vtol_position_controller(void)
// allow up to the WP speed when we are further away, slowing to the pos2 target speed // allow up to the WP speed when we are further away, slowing to the pos2 target speed
// when we are close // when we are close
target_speed = linear_interpolate(position2_target_speed, scaled_wp_speed, target_speed = linear_interpolate(position2_target_speed, wp_speed,
distance, distance,
position2_dist_threshold*1.5, position2_dist_threshold*1.5,
2*position2_dist_threshold + stopping_distance(rel_groundspeed_sq)); 2*position2_dist_threshold + stopping_distance(rel_groundspeed_sq));
@ -2530,11 +2530,15 @@ void QuadPlane::vtol_position_controller(void)
target_speed_xy_cms = diff_wp_norm * target_speed * 100; target_speed_xy_cms = diff_wp_norm * target_speed * 100;
have_target_yaw = true; 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 Vector2f wind = plane.ahrs.wind_estimate().xy();
const float gnd_speed = plane.ahrs.groundspeed(); const float gnd_speed = plane.ahrs.groundspeed();
Vector2f target_speed_xy = landing_velocity + diff_wp_norm * gnd_speed - wind; Vector2f target_speed_xy = landing_velocity + diff_wp_norm * gnd_speed - wind;
target_yaw_deg = degrees(target_speed_xy.angle()); 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; const float target_speed_ms = target_speed_xy_cms.length() * 0.01;