implemented basic heading timeout for waypoint acceptance, added parameter for yaw error on waypoint heading acceptance, set yaw timeout for vtol default

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -47,6 +47,7 @@ then
 	param set MPC_LAND_SPEED 0.7
 	param set MPC_Z_VEL_MAX 1.5
 	param set MPC_XY_VEL_MAX 4.0
+	param set MIS_YAW_TMT 10
 fi
 
 set PWM_DISARMED 900

posix-configs/SITL/init/rcS_gazebo_standard_vtol

@@ -32,6 +32,7 @@ param set SENS_BOARD_ROT 8
 param set COM_RC_IN_MODE 1
 param set NAV_ACC_RAD 3.0
 param set MPC_TKO_SPEED 1.0
+param set MIS_YAW_TMT 5
 rgbledsim start
 tone_alarm start
 gyrosim start

src/modules/navigator/mission_block.cpp

@@ -49,6 +49,7 @@
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <mavlink/mavlink_log.h>
+#include <mathlib/mathlib.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_controls.h>
@@ -68,9 +69,12 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
 	_action_start(0),
+	_time_wp_reached(0),
 	_actuators{},
 	_actuator_pub(nullptr),
 	_cmd_pub(nullptr),
+	_param_yaw_timeout(this, "MIS_YAW_TMT", false),
+	_param_yaw_err(this, "MIS_YAW_ERR", false),
 	_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
 	_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false)
 {
@@ -174,6 +178,11 @@ MissionBlock::is_mission_item_reached()
 				_waypoint_position_reached = true;
 			}
 		}
+
+		if (_waypoint_position_reached) {
+			// reached just now
+			_time_wp_reached = now;
+		}
 	}
 
 	/* Check if the waypoint and the requested yaw setpoint. */
@@ -186,7 +195,9 @@ MissionBlock::is_mission_item_reached()
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
 
-			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
+			if (fabsf(yaw_err) < math::radians(_param_yaw_err.get())
+					|| (_param_yaw_timeout.get() >= -FLT_EPSILON &&
+						now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) {
 				_waypoint_yaw_reached = true;
 			}
 
@@ -221,6 +232,7 @@ MissionBlock::reset_mission_item_reached()
 	_waypoint_position_reached = false;
 	_waypoint_yaw_reached = false;
 	_time_first_inside_orbit = 0;
+	_time_wp_reached = 0;
 }
 
 void

src/modules/navigator/mission_block.h

@@ -45,9 +45,6 @@
 
 #include <navigator/navigation.h>
 
-#include <controllib/blocks.hpp>
-#include <controllib/block/BlockParam.hpp>
-
 #include <uORB/topics/mission.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
@@ -128,10 +125,14 @@ protected:
 	bool _waypoint_yaw_reached;
 	hrt_abstime _time_first_inside_orbit;
 	hrt_abstime _action_start;
+	hrt_abstime _time_wp_reached;
 
 	actuator_controls_s _actuators;
 	orb_advert_t    _actuator_pub;
 	orb_advert_t	_cmd_pub;
+
+	control::BlockParamFloat _param_yaw_timeout;
+	control::BlockParamFloat _param_yaw_err;
 	control::BlockParamInt _param_vtol_wv_land;
 	control::BlockParamInt _param_vtol_wv_loiter;
 };

src/modules/navigator/mission_params.c

@@ -107,3 +107,25 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 1);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_YAWMODE, 1);
+
+/**
+ * Time in seconds we wait on reaching target heading at a waypoint.
+ *
+ * Prevents missions getting blocked at waypoints because it cannot reach target yaw.
+ * Mainly useful for VTOLs that have less yaw authority and might not reach target
+ * yaw in wind. Disabled by default. Can be set to 0 if it should not care for yaw on waypoints.
+ *
+ * @min -1
+ * @max 20
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f);
+
+/**
+ * Max yaw error in degree needed for waypoint heading acceptance.
+ *
+ * @min 0
+ * @max 90
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);