fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-05 13:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improve FW landing predictability

Browse Source 
- Use the course over ground as loiter exit criteria (better wind behavior)
 - Compute the tangent origin coordinate explicitly instead of using immediate position
This commit is contained in:
Nate Weibley
2017-05-04 07:51:46 +02:00
committed by Lorenz Meier
parent 3d2d6c4fdc
commit 50740ef813
1 changed files with 24 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+24 -5
View File
@@ -323,8 +323,11 @@ MissionBlock::is_mission_item_reached()
|| (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
&& PX4_ISFINITE(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
/* check course if defined only for rotary wing except takeoff */
float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f(
_navigator->get_global_position()->vel_e,
_navigator->get_global_position()->vel_n);
float yaw_err = _wrap_pi(_mission_item.yaw - cog);
/* accept yaw if reached or if timeout is set in which case we ignore not forced headings */
if (fabsf(yaw_err) < math::radians(_param_yaw_err.get())
@@ -362,10 +365,26 @@ MissionBlock::is_mission_item_reached()
(_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
// reset lat/lon of loiter waypoint so vehicle exits on a tangent
// reset lat/lon of loiter waypoint so vehicle follows a tangent
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
curr_sp->lat = _navigator->get_global_position()->lat;
curr_sp->lon = _navigator->get_global_position()->lon;
const struct position_setpoint_s *next_sp = &_navigator->get_position_setpoint_triplet()->next;
float range = get_distance_to_next_waypoint(curr_sp->lat, curr_sp->lon, next_sp->lat, next_sp->lon);
float bearing = get_bearing_to_next_waypoint(curr_sp->lat, curr_sp->lon, next_sp->lat, next_sp->lon);
float inner_angle = M_PI_2_F - asinf(_mission_item.loiter_radius / range);
// Compute "ideal" tangent origin
if (curr_sp->loiter_direction > 0) {
bearing -= inner_angle;
} else {
bearing += inner_angle;
}
// Replace current setpoint lat/lon with tangent coordinate
waypoint_from_heading_and_distance(curr_sp->lat, curr_sp->lon,
bearing, curr_sp->loiter_radius,
&curr_sp->lat, &curr_sp->lon);
}
return true;