Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: in overshoot allow up to the Q WP speed

Browse Source 
this prevents us using too much battery when we are a long way from
the landing point
This commit is contained in:
Andrew Tridgell 2022-07-30 11:45:11 +10:00 committed by Randy Mackay
parent bc115da98d
commit a51071678f
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/quadplane.cpp
View File

@ -2519,13 +2519,21 @@ void QuadPlane::vtol_position_controller(void)
to our position2 threshold speed to our position2 threshold speed
*/ */
target_accel_cms.zero(); target_accel_cms.zero();
target_speed_xy_cms = diff_wp_norm * position2_target_speed * 100;
// 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,
distance,
position2_dist_threshold*1.5,
2*position2_dist_threshold + stopping_distance(rel_groundspeed_sq));
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
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 = 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());
} }
} }