fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Reposition: Do not yaw for small dispsplacements

Browse Source
This commit is contained in:
Lorenz Meier
2016-05-03 13:35:53 +02:00
parent a4ad826958
commit e1e15add01
1 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/loiter.cpp
+11 -6
View File
@@ -121,12 +121,17 @@ Loiter::reposition()
// set yaw
// calculate direction the vehicle should point to.
pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon);
float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
if (travel_dist > 1.0f) {
// calculate direction the vehicle should point to.
pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon);
}
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);