diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp
index 5b6f5f2ffc..38ed565a05 100644
--- a/src/lib/tecs/TECS.cpp
+++ b/src/lib/tecs/TECS.cpp
@@ -156,6 +156,10 @@ void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceS
 	_alt_control_traj_generator.setMaxAccel(param.vert_accel_limit);
 	_alt_control_traj_generator.setMaxVel(fmax(param.max_climb_rate, param.max_sink_rate));
 
+	// XXX: this is a bit risky.. .alt_rate here could be NAN (by interface design) - and is only ok to input to the
+	// setVelSpFeedback() method because it calls the reset in the logic below when it is NAN.
+	// TODO: stop it with the NAN interfaces, make sure to take care of this when refactoring and separating altitude
+	// and height rate control loops.
 	_velocity_control_traj_generator.setVelSpFeedback(setpoint.alt_rate);
 
 	bool control_altitude = true;
diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
index 891b86e5ea..9f7c05ce1e 100644
--- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
@@ -1995,6 +1995,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 	}
 
 	/* heading control */
+	// TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here)
 	if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH &&
 	    fabsf(_manual_control_setpoint.yaw) < HDG_HOLD_MAN_INPUT_THRESH) {
 
@@ -2019,28 +2020,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 				_hdg_hold_enabled = true;
 				_hdg_hold_yaw = _yaw;
 
-				get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
+				_hdg_hold_position.lat = _current_latitude;
+				_hdg_hold_position.lon = _current_longitude;
 			}
 
-			/* we have a valid heading hold position, are we too close? */
-			const float dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _hdg_hold_curr_wp.lat,
-					   _hdg_hold_curr_wp.lon);
-
-			if (dist < HDG_HOLD_REACHED_DIST) {
-				get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
-			}
-
-			Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
-			Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};
-
 			Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-			Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
-						 prev_wp(1));
+			Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon);
 
 			_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
 			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+			navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel);
 			_att_sp.roll_body = _npfg.getRollSetpoint();
 			calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
 
@@ -2049,7 +2038,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 	}
 
 	tecs_update_pitch_throttle(control_interval,
-				   _current_altitude,
+				   _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude.
 				   calibrated_airspeed_sp,
 				   min_pitch,
 				   radians(_param_fw_p_lim_max.get()),
diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
index a2fb6cd2c2..1fccd36099 100644
--- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
@@ -287,8 +287,7 @@ private:
 	bool _hdg_hold_enabled{false}; // heading hold enabled
 	bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
 
-	position_setpoint_s _hdg_hold_prev_wp{}; // position where heading hold started
-	position_setpoint_s _hdg_hold_curr_wp{}; // position to which heading hold flies
+	position_setpoint_s _hdg_hold_position{}; // position where heading hold started
 
 	// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
 	float _manual_control_setpoint_for_height_rate{0.0f};