transition alignment will force heading now and go to RTL if it cannot reach it in time, handle mission failure correctly, reset after mission update, issue message with actual problem

src/modules/commander/commander.cpp

@@ -2043,6 +2043,15 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+
+			if (status.mission_failure != mission_result.mission_failure) {
+				status.mission_failure = mission_result.mission_failure;
+				status_changed = true;
+
+				if (status.mission_failure) {
+					mavlink_log_critical(mavlink_fd, "mission cannot be completed");
+				}
+			}
 		}
 
 		/* start geofence result check */

src/modules/commander/state_machine_helper.cpp

@@ -659,6 +659,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (status->vtol_transition_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+		} else if (status->mission_failure) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
 		/* datalink loss enabled:
 		 * check for datalink lost: this should always trigger RTGS */

src/modules/navigator/mission.cpp

@@ -219,6 +219,8 @@ Mission::update_onboard_mission()
 
 		// XXX check validity here as well
 		_navigator->get_mission_result()->valid = true;
+		/* reset mission failure if we have an updated valid mission */
+		_navigator->get_mission_result()->mission_failure = false;
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
 
@@ -263,6 +265,10 @@ Mission::update_offboard_mission()
 				_navigator->get_vstatus()->condition_landed);
 
 		_navigator->get_mission_result()->valid = !failed;
+		if (!failed) {
+			/* reset mission failure if we have an updated valid mission */
+			_navigator->get_mission_result()->mission_failure = false;
+		}
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
 
@@ -510,6 +516,7 @@ Mission::set_mission_items()
 				_navigator->get_global_position()->lon,
 				mission_item_next_position.lat,
 				mission_item_next_position.lon);
+			_mission_item.force_heading = true;
 		}
 
 		/* yaw is aligned now */

src/modules/navigator/mission_block.cpp

@@ -196,9 +196,17 @@ MissionBlock::is_mission_item_reached()
 			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
 
 			if (fabsf(yaw_err) < math::radians(_param_yaw_err.get())
+					/* check if we should accept heading after a timeout, 0 means accept instantly */
 					|| (_param_yaw_timeout.get() >= -FLT_EPSILON &&
 						now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) {
-				_waypoint_yaw_reached = true;
+
+				/* if heading needs to be reached but we got here because of the timeout, abort mission */
+				if (_mission_item.force_heading && !(fabsf(yaw_err) < math::radians(_param_yaw_err.get()))) {
+					_navigator->set_mission_failure("unable to reach heading within timeout");
+
+				} else {
+					_waypoint_yaw_reached = true;
+				}
 			}
 
 		} else {

src/modules/navigator/mission_params.c

@@ -113,7 +113,8 @@ PARAM_DEFINE_INT32(MIS_YAWMODE, 1);
  *
  * 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.
+ * yaw in wind. Disabled by default. If set to 0 it will not wait for yaw to align,
+ * however VTOL transitions will abort if heading is not reached.
  *
  * @min -1
  * @max 20

src/modules/navigator/navigation.h

@@ -105,6 +105,7 @@ struct mission_item_s {
 	unsigned do_jump_current_count;	/**< count how many times the jump has been done	*/
 	float params[7];		/**< array to store mission command values for MAV_FRAME_MISSION ***/
 	int8_t frame;			/**< mission frame ***/
+	bool force_heading;		/**< heading needs to be reached ***/
 };
 #pragma pack(pop)
 #include <uORB/topics/mission.h>

src/modules/navigator/navigator.h

@@ -168,6 +168,8 @@ public:
 
 	void		increment_mission_instance_count() { _mission_instance_count++; }
 
+	void 		set_mission_failure(const char *reason);
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */

src/modules/navigator/navigator_main.cpp

@@ -750,3 +750,13 @@ Navigator::publish_att_sp()
 		_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 	}
 }
+
+void
+Navigator::set_mission_failure(const char* reason)
+{
+	if (!_mission_result.mission_failure) {
+		_mission_result.mission_failure = true;
+		set_mission_result_updated();
+		mavlink_log_critical(_mavlink_fd, "%s", reason);
+	}
+}