diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 07d765c3bc..0d09e8526b 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -310,7 +310,7 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou
 		waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
 	} else {
 		heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
-		heading = _wrap_2pi(heading + 180.0f * M_PI_F);
+		heading = _wrap_2pi(heading + M_PI_F);
 		waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
 	}
 }
@@ -318,7 +318,7 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou
 __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end)
 {
 	bearing = _wrap_2pi(bearing);
-	double radius_ratio = (double)(dist / CONSTANTS_RADIUS_OF_EARTH);
+	double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
 
 	*lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing));
 	*lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end));
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 7a98859f81..080b96c5ce 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
@@ -1264,9 +1264,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon);
 			curr_wp_shifted(0) = (float)lat;
 			curr_wp_shifted(1) = (float)lon;
+
 			// we want the plane to keep tracking the desired flight path until we start flaring
 			// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
-			if (land_noreturn_vertical) {
+			//if (land_noreturn_vertical) {
+			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
 
 				/* heading hold, along the line connecting this and the last waypoint */
 
@@ -1288,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			} else {
 
 				/* normal navigation */
-				_l1_control.navigate_waypoints(prev_wp, curr_wp_shifted, current_position, ground_speed_2d);
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			}
 
 			_att_sp.roll_body = _l1_control.nav_roll();
@@ -1405,7 +1407,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					land_noreturn_vertical = true;
 				} else {
 					if (_global_pos.vel_d > 0.1f) {
-						_att_sp.pitch_body = _parameters.land_flare_pitch_min_deg * (height_flare - (_global_pos.alt - terrain_alt)) / height_flare;
+						_att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) *
+								math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f);
 					} else {
 						_att_sp.pitch_body = _att_sp.pitch_body;
 					}
@@ -1859,12 +1862,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* During a takeoff waypoint while waiting for launch the pitch sp is set
 	 * already (not by tecs) */
-	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
-				pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
+	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO && (
+				(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 					(launch_detection_state == LAUNCHDETECTION_RES_NONE ||
-					_runway_takeoff.runwayTakeoffEnabled() ||
-					land_noreturn_vertical)
-				)) {
+					_runway_takeoff.runwayTakeoffEnabled())) ||
+				(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
+					land_noreturn_vertical))
+	)) {
 		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}