mc_pos_control: change use avoidance to handle data loss during mission

There were accidents because when missions lead through an obstacle
and it should be avoided but the setpoints from the external obstacle
avoidance module are suddenly missing the mission is continued into the
obstacle which results in a crash.

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1211,12 +1211,22 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
 bool
 MulticopterPositionControl::use_obstacle_avoidance()
 {
-	/* check that external obstacle avoidance is sending data and that the first point is valid */
-	return (MPC_OBS_AVOID.get()
-		&& (hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) < TRAJECTORY_STREAM_TIMEOUT_US)
-		&& (_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true)
-		&& ((_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) ||
-		    (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)));
+	if (MPC_OBS_AVOID.get()) {
+		const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
+		const bool avoidance_point_valid = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
+		const bool in_mission = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+		const bool in_rtl = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+		// switch to hold mode to stop when we loose external avoidance data during a mission
+		if (avoidance_data_timeout && in_mission) {
+			send_vehicle_cmd_do(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+		}
+
+		if ((in_mission || in_rtl) && !avoidance_data_timeout && avoidance_point_valid) {
+			return true;
+		}
+	}
+	return false;
 }
 
 void