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

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -47,7 +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
+	param set MIS_YAW_TMT 5
 fi
 
 set PWM_DISARMED 900

msg/mission_result.msg

@@ -10,3 +10,4 @@ bool flight_termination	# true if the navigator demands a flight termination fro
 bool item_do_jump_changed	# true if the number of do jumps remaining has changed
 uint32 item_changed_index	# indicate which item has changed
 uint32 item_do_jump_remaining	# set to the number of do jumps remaining for that item
+bool mission_failure		# true if the mission cannot continue or be completed for some reason

msg/vehicle_status.msg

@@ -154,7 +154,7 @@ bool vtol_transition_failure		# Set to true if vtol transition failed
 bool vtol_transition_failure_cmd	# Set to true if vtol transition failure mode is commanded
 bool gps_failure				# Set to true if a gps failure is detected
 bool gps_failure_cmd				# Set to true if a gps failure mode is commanded
-
+bool mission_failure				# Set to true if mission could not continue/finish
 bool barometer_failure				# Set to true if a barometer failure is detected
 
 bool offboard_control_signal_found_once

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,10 +196,18 @@ 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)) {
+
+				/* 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 {
 			_waypoint_yaw_reached = true;

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);
+	}
+}