From 1f3c5d00e43b1aebe765290e138094b9f59d4577 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 4 Jun 2015 08:28:43 -0600 Subject: [PATCH] fix posctl heading hold --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f1f09473eb..93244b0bb4 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -912,8 +912,8 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa if (flag_init) { // on init set first waypoint to momentary position - waypoint_prev.lat = _global_pos.lat; - waypoint_prev.lon = _global_pos.lon; + waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; } else { /* for previous waypoint use the one still in front of us but shift it such that it is