fixed wing position control:

- only lock on yaw if yawrate is below threshold
- remove more magic numbers

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -94,10 +94,13 @@
 #include <platforms/px4_defines.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
+#define HDG_HOLD_DIST_NEXT 			3000.0f 	// initial distance of waypoint in front of plane in heading hold mode
+#define HDG_HOLD_REACHED_DIST 		1000.0f 	// distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
+#define HDG_HOLD_SET_BACK_DIST 		100.0f 		// distance by which previous waypoint is set behind the plane
+#define HDG_HOLD_YAWRATE_THRESH 	0.1f 		// max yawrate at which plane locks yaw for heading hold mode
+#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 	// max manual roll input from user which does not change the locked heading
 
-#define HDG_HOLD_DIST_NEXT 		3000.0f
-#define HDG_HOLD_REACHED_DIST 	1000.0f
-#define HDG_HOLD_SET_BACK_DIST 	100.0f
+#define THROTTLE_THRESH 0.05f 	// max throttle from user which will not lead to motors spinning up in altitude controlled modes
 
 
 /**
@@ -1398,7 +1401,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* throttle limiting */
 		throttle_max = _parameters.throttle_max;
-		if (fabsf(_manual.z) < 0.05f) {
+		if (fabsf(_manual.z) < THROTTLE_THRESH) {
 			throttle_max = 0.0f;
 		}
 
@@ -1418,7 +1421,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* heading control */
 
-		if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
+		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH) {
 			/* heading / roll is zero, lock onto current heading */
 
 			/* just switched back from non heading-hold to heading hold */
@@ -1482,7 +1485,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* throttle limiting */
 		throttle_max = _parameters.throttle_max;
-		if (fabsf(_manual.z) < 0.05f) {
+		if (fabsf(_manual.z) < THROTTLE_THRESH) {
 			throttle_max = 0.0f;
 		}