Only check mission when needed

src/modules/navigator/mission.cpp

@@ -81,6 +81,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_need_takeoff(true),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
+	_home_inited(false),
 	_need_mission_reset(false),
 	_missionFeasibilityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
@@ -108,12 +109,14 @@ Mission::on_inactive()
 		/* check if missions have changed so that feedback to ground station is given */
 		bool onboard_updated = false;
 		orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+
 		if (onboard_updated) {
 			update_onboard_mission();
 		}
 
 		bool offboard_updated = false;
 		orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+
 		if (offboard_updated) {
 			update_offboard_mission();
 		}
@@ -143,7 +146,7 @@ Mission::on_inactive()
 		}
 
 		/* On init let's check the mission, maybe there is already one available. */
-		check_mission_valid();
+		check_mission_valid(false);
 
 		_inited = true;
 	}
@@ -151,6 +154,7 @@ Mission::on_inactive()
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
 		_need_takeoff = true;
+
 		/* Reset work item type to default if auto take-off has been paused or aborted,
 		   and we landed in manual mode. */
 		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
@@ -168,17 +172,19 @@ Mission::on_activation()
 void
 Mission::on_active()
 {
-	check_mission_valid();
+	check_mission_valid(false);
 
 	/* check if anything has changed */
 	bool onboard_updated = false;
 	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+
 	if (onboard_updated) {
 		update_onboard_mission();
 	}
 
 	bool offboard_updated = false;
 	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+
 	if (offboard_updated) {
 		update_offboard_mission();
 	}
@@ -198,6 +204,7 @@ Mission::on_active()
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
 		set_mission_item_reached();
+
 		if (_mission_item.autocontinue) {
 			/* switch to next waypoint if 'autocontinue' flag set */
 			advance_mission();
@@ -216,9 +223,9 @@ Mission::on_active()
 
 	/* see if we need to update the current yaw heading */
 	if ((_param_yawmode.get() != MISSION_YAWMODE_NONE
-			&& _param_yawmode.get() < MISSION_YAWMODE_MAX
+	     && _param_yawmode.get() < MISSION_YAWMODE_MAX
-			&& _mission_type != MISSION_TYPE_NONE)
+	     && _mission_type != MISSION_TYPE_NONE)
-			|| _navigator->get_vstatus()->is_vtol) {
+	    || _navigator->get_vstatus()->is_vtol) {
 		heading_sp_update();
 	}
 
@@ -229,17 +236,20 @@ Mission::update_onboard_mission()
 {
 	if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
 		/* accept the current index set by the onboard mission if it is within bounds */
-		if (_onboard_mission.current_seq >=0
+		if (_onboard_mission.current_seq >= 0
-		&& _onboard_mission.current_seq < (int)_onboard_mission.count) {
+		    && _onboard_mission.current_seq < (int)_onboard_mission.count) {
 			_current_onboard_mission_index = _onboard_mission.current_seq;
+
 		} else {
 			/* if less WPs available, reset to first WP */
 			if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
 				_current_onboard_mission_index = 0;
-			/* if not initialized, set it to 0 */
+				/* if not initialized, set it to 0 */
+
 			} else if (_current_onboard_mission_index < 0) {
 				_current_onboard_mission_index = 0;
 			}
+
 			/* otherwise, just leave it */
 		}
 
@@ -263,23 +273,28 @@ Mission::update_offboard_mission()
 	bool failed = true;
 
 	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
-		warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
+		warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id,
+		      _offboard_mission.count, _offboard_mission.current_seq);
+
 		/* determine current index */
 		if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
 			_current_offboard_mission_index = _offboard_mission.current_seq;
+
 		} else {
 			/* if less items available, reset to first item */
 			if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
 				_current_offboard_mission_index = 0;
 
-			/* if not initialized, set it to 0 */
+				/* if not initialized, set it to 0 */
+
 			} else if (_current_offboard_mission_index < 0) {
 				_current_offboard_mission_index = 0;
 			}
+
 			/* otherwise, just leave it */
 		}
 
-		check_mission_valid();
+		check_mission_valid(true);
 
 		failed = !_navigator->get_mission_result()->valid;
 
@@ -332,6 +347,7 @@ Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
 {
 	if (_mission_item.altitude_is_relative) {
 		return _mission_item.altitude + _navigator->get_home_position()->alt;
+
 	} else {
 		return _mission_item.altitude;
 	}
@@ -364,28 +380,34 @@ Mission::set_mission_items()
 	}
 
 	/* try setting onboard mission item */
-	if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
+	if (_param_onboard_enabled.get()
+	    && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission");
 			user_feedback_done = true;
 		}
+
 		_mission_type = MISSION_TYPE_ONBOARD;
 
-	/* try setting offboard mission item */
+		/* try setting offboard mission item */
+
 	} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission");
 			user_feedback_done = true;
 		}
+
 		_mission_type = MISSION_TYPE_OFFBOARD;
+
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
 
 			if (_navigator->get_land_detected()->landed) {
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, landed");
+
 			} else {
 				/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
@@ -422,6 +444,7 @@ Mission::set_mission_items()
 				/* landed, refusing to take off without a mission */
 
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
+
 			} else {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");
 			}
@@ -443,7 +466,7 @@ Mission::set_mission_items()
 
 		/* force vtol land */
 		if (_mission_item.nav_cmd == NAV_CMD_LAND && _navigator->get_vstatus()->is_vtol
-			&& _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) {
+		    && _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) {
 
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
@@ -463,7 +486,8 @@ Mission::set_mission_items()
 
 			float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home",
+					 (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
 			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
 			_mission_item.lat = _navigator->get_global_position()->lat;
@@ -480,13 +504,14 @@ Mission::set_mission_items()
 		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
 
 			if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
-					&& _navigator->get_vstatus()->is_rotary_wing
+			    && _navigator->get_vstatus()->is_rotary_wing
-					&& !_navigator->get_land_detected()->landed
+			    && !_navigator->get_land_detected()->landed
-					&& has_next_position_item) {
+			    && has_next_position_item) {
 
 				/* check if the vtol_takeoff command is on top of us */
-				if (do_need_move_to_takeoff()){
+				if (do_need_move_to_takeoff()) {
 					new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
+
 				} else {
 					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 				}
@@ -495,6 +520,7 @@ Mission::set_mission_items()
 				_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 				_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
 				_mission_item.yaw = _navigator->get_global_position()->yaw;
+
 			} else {
 				new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -506,7 +532,7 @@ Mission::set_mission_items()
 
 		/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
 		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
-				&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
+		    && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
 
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -516,14 +542,15 @@ Mission::set_mission_items()
 
 		/* move to land wp as fixed wing */
 		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-				&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
+		    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
-				&& !_navigator->get_land_detected()->landed) {
+		    && !_navigator->get_land_detected()->landed) {
 
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 			/* use current mission item as next position item */
 			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
 			has_next_position_item = true;
 			float altitude = _navigator->get_global_position()->alt;
+
 			if (pos_sp_triplet->current.valid) {
 				altitude = pos_sp_triplet->current.alt;
 			}
@@ -537,9 +564,9 @@ Mission::set_mission_items()
 
 		/* transition to MC */
 		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-				&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
+		    && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
-				&& !_navigator->get_vstatus()->is_rotary_wing
+		    && !_navigator->get_vstatus()->is_rotary_wing
-				&& !_navigator->get_land_detected()->landed) {
+		    && !_navigator->get_land_detected()->landed) {
 
 			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
@@ -549,8 +576,8 @@ Mission::set_mission_items()
 
 		/* move to landing waypoint before descent if necessary */
 		if (do_need_move_to_land() &&
-				(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
+		    (_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
-				 _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
+		     _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
 
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
@@ -576,7 +603,7 @@ Mission::set_mission_items()
 
 		/* we just moved to the landing waypoint, now descend */
 		if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
-				&& _navigator->get_vstatus()->is_rotary_wing) {
+		    && _navigator->get_vstatus()->is_rotary_wing) {
 
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
@@ -586,19 +613,20 @@ Mission::set_mission_items()
 		/* ignore yaw for landing items */
 		/* XXX: if specified heading for landing is desired we could add another step before the descent
 		 * that aligns the vehicle first */
-		if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND ) {
+		if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
 			_mission_item.yaw = NAN;
 		}
 
-	/* handle non-position mission items such as commands */
+		/* handle non-position mission items such as commands */
+
 	} else {
 
 		/* turn towards next waypoint before MC to FW transition */
 		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
-				&& _work_item_type != WORK_ITEM_TYPE_ALIGN
+		    && _work_item_type != WORK_ITEM_TYPE_ALIGN
-				&& _navigator->get_vstatus()->is_rotary_wing
+		    && _navigator->get_vstatus()->is_rotary_wing
-				&& !_navigator->get_land_detected()->landed
+		    && !_navigator->get_land_detected()->landed
-				&& has_next_position_item) {
+		    && has_next_position_item) {
 
 			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
 			set_align_mission_item(&_mission_item, &mission_item_next_position);
@@ -624,7 +652,7 @@ Mission::set_mission_items()
 
 	/* require takeoff after landing or idle */
 	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
-			|| pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+	    || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 
 		_need_takeoff = true;
 	}
@@ -635,6 +663,7 @@ Mission::set_mission_items()
 	if (_mission_type == MISSION_TYPE_OFFBOARD) {
 		set_current_offboard_mission_item();
 	}
+
 	// TODO: report onboard mission item somehow
 
 	if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
@@ -643,6 +672,7 @@ Mission::set_mission_items()
 		if (has_next_position_item) {
 			/* got next mission item, update setpoint triplet */
 			mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
+
 		} else {
 			/* next mission item is not available */
 			pos_sp_triplet->next.valid = false;
@@ -657,10 +687,10 @@ Mission::set_mission_items()
 	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
 
 		_distance_current_previous = get_distance_to_next_waypoint(
-				pos_sp_triplet->current.lat,
+						     pos_sp_triplet->current.lat,
-				pos_sp_triplet->current.lon,
+						     pos_sp_triplet->current.lon,
-				pos_sp_triplet->previous.lat,
+						     pos_sp_triplet->previous.lat,
-				pos_sp_triplet->previous.lon);
+						     pos_sp_triplet->previous.lon);
 	}
 
 	_navigator->set_position_setpoint_triplet_updated();
@@ -677,19 +707,20 @@ Mission::do_need_takeoff()
 		if (_navigator->get_land_detected()->landed) {
 			_need_takeoff = true;
 
-		/* if in-air and already above takeoff height, don't do takeoff */
+			/* if in-air and already above takeoff height, don't do takeoff */
+
 		} else if (_navigator->get_global_position()->alt > takeoff_alt) {
 			_need_takeoff = false;
 		}
 
 		/* check if current mission item is one that requires takeoff before */
 		if (_need_takeoff && (
-				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+			    _mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
+			    _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+			    _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+			    _mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
+			    _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
-				_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
+			    _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
 
 			_need_takeoff = false;
 			return true;
@@ -703,10 +734,10 @@ bool
 Mission::do_need_move_to_land()
 {
 	if (_navigator->get_vstatus()->is_rotary_wing
-		&& (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
+	    && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
 
 		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+				  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
 
 		return d_current > _navigator->get_acceptance_radius();
 	}
@@ -720,7 +751,7 @@ Mission::do_need_move_to_takeoff()
 	if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
 
 		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+				  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
 
 		return d_current > _navigator->get_acceptance_radius();
 	}
@@ -754,10 +785,10 @@ Mission::set_align_mission_item(struct mission_item_s *mission_item, struct miss
 	mission_item->autocontinue = true;
 	mission_item->time_inside = 0;
 	mission_item->yaw = get_bearing_to_next_waypoint(
-		_navigator->get_global_position()->lat,
+				    _navigator->get_global_position()->lat,
-		_navigator->get_global_position()->lon,
+				    _navigator->get_global_position()->lon,
-		mission_item_next->lat,
+				    mission_item_next->lat,
-		mission_item_next->lon);
+				    mission_item_next->lon);
 	mission_item->force_heading = true;
 }
 
@@ -783,11 +814,11 @@ Mission::heading_sp_update()
 {
 	/* we don't want to be yawing during takeoff, landing or aligning for a transition */
 	if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
-			|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
+	    || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
-			|| _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
+	    || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
-			|| _mission_item.nav_cmd == NAV_CMD_LAND
+	    || _mission_item.nav_cmd == NAV_CMD_LAND
-			|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
+	    || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-			|| _work_item_type == WORK_ITEM_TYPE_ALIGN) {
+	    || _work_item_type == WORK_ITEM_TYPE_ALIGN) {
 		return;
 	}
 
@@ -815,32 +846,33 @@ Mission::heading_sp_update()
 
 		/* target location is home */
 		if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME
-				|| _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
+		     || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
-				// need to be rotary wing for this but not in a transition
+		    // need to be rotary wing for this but not in a transition
-				// in VTOL mode this will prevent updating yaw during FW flight
+		    // in VTOL mode this will prevent updating yaw during FW flight
-				// (which would result in a wrong yaw setpoint spike during back transition)
+		    // (which would result in a wrong yaw setpoint spike during back transition)
-				&& _navigator->get_vstatus()->is_rotary_wing
+		    && _navigator->get_vstatus()->is_rotary_wing
-				&& !(_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _navigator->get_vstatus()->in_transition_mode)) {
+		    && !(_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _navigator->get_vstatus()->in_transition_mode)) {
 			point_to_latlon[0] = _navigator->get_home_position()->lat;
 			point_to_latlon[1] = _navigator->get_home_position()->lon;
 
-		/* target location is next (current) waypoint */
+			/* target location is next (current) waypoint */
+
 		} else {
 			point_to_latlon[0] = pos_sp_triplet->current.lat;
 			point_to_latlon[1] = pos_sp_triplet->current.lon;
 		}
 
 		float d_current = get_distance_to_next_waypoint(
-			point_from_latlon[0], point_from_latlon[1],
+					  point_from_latlon[0], point_from_latlon[1],
-			point_to_latlon[0], point_to_latlon[1]);
+					  point_to_latlon[0], point_to_latlon[1]);
 
 		/* stop if positions are close together to prevent excessive yawing */
 		if (d_current > _navigator->get_acceptance_radius()) {
 			float yaw = get_bearing_to_next_waypoint(
-				point_from_latlon[0],
+					    point_from_latlon[0],
-				point_from_latlon[1],
+					    point_from_latlon[1],
-				point_to_latlon[0],
+					    point_to_latlon[0],
-				point_to_latlon[1]);
+					    point_to_latlon[1]);
 
 			/* always keep the back of the rotary wing pointing towards home */
 			if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
@@ -866,7 +898,7 @@ Mission::altitude_sp_foh_update()
 
 	/* Don't change setpoint if last and current waypoint are not valid */
 	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
-			!PX4_ISFINITE(_mission_item_previous_alt)) {
+	    !PX4_ISFINITE(_mission_item_previous_alt)) {
 		return;
 	}
 
@@ -881,21 +913,21 @@ Mission::altitude_sp_foh_update()
 	 * NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
 	 * */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND
-			|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
+	    || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-			|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
+	    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF
-			|| _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
+	    || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
-			|| !_navigator->is_planned_mission()) {
+	    || !_navigator->is_planned_mission()) {
 		return;
 	}
 
 	/* Calculate distance to current waypoint */
 	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+			  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
 
 	/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
 	 * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
 	_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
-			_distance_current_previous);
+						_distance_current_previous);
 
 	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
 	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
@@ -909,7 +941,8 @@ Mission::altitude_sp_foh_update()
 		* radius around the current waypoint
 		**/
 		float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt);
-		float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius));
+		float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius(
+						   _mission_item.acceptance_radius));
 		float a = _mission_item_previous_alt - grad * _distance_current_previous;
 		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
 	}
@@ -926,7 +959,7 @@ Mission::altitude_sp_foh_reset()
 
 bool
 Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
-	struct mission_item_s *next_position_mission_item, bool *has_next_position_item)
+			       struct mission_item_s *next_position_mission_item, bool *has_next_position_item)
 {
 	bool first_res = false;
 	int offset = 1;
@@ -936,7 +969,7 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
 		first_res = true;
 
 		/* trying to find next position mission item */
-		while(read_mission_item(onboard, offset, next_position_mission_item)) {
+		while (read_mission_item(onboard, offset, next_position_mission_item)) {
 
 			if (item_contains_position(next_position_mission_item)) {
 				*has_next_position_item = true;
@@ -984,8 +1017,10 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
 			/* mission item index out of bounds - if they are equal, we just reached the end */
 			if (*mission_index_ptr != (int)mission->count) {
-				mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
+				mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "[wpm] err: index: %d, max: %d", *mission_index_ptr,
+								 (int)mission->count);
 			}
+
 			return false;
 		}
 
@@ -1011,17 +1046,20 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 				* but not for the read ahead mission item */
 				if (offset == 0) {
 					(mission_item_tmp.do_jump_current_count)++;
+
 					/* save repeat count */
 					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
-						&mission_item_tmp, len) != len) {
+						     &mission_item_tmp, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
+
 					report_do_jump_mission_changed(*mission_index_ptr,
-									   mission_item_tmp.do_jump_repeat_count);
+								       mission_item_tmp.do_jump_repeat_count);
 				}
+
 				/* set new mission item index and repeat
 				* we don't have to validate here, if it's invalid, we should realize this later .*/
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
@@ -1030,6 +1068,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 				if (offset == 0) {
 					mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed");
 				}
+
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
 				(*mission_index_ptr)++;
 			}
@@ -1062,7 +1101,8 @@ Mission::save_offboard_mission_state()
 		if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) {
 			/* navigator may modify only sequence, write modified state only if it changed */
 			if (mission_state.current_seq != _current_offboard_mission_index) {
-				if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+				if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state,
+					     sizeof(mission_s)) != sizeof(mission_s)) {
 					warnx("ERROR: can't save mission state");
 					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state");
 				}
@@ -1079,7 +1119,8 @@ Mission::save_offboard_mission_state()
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: invalid mission state");
 
 		/* write modified state only if changed */
-		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state,
+			     sizeof(mission_s)) != sizeof(mission_s)) {
 			warnx("ERROR: can't save mission state");
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state");
 		}
@@ -1127,27 +1168,31 @@ Mission::set_mission_finished()
 }
 
 void
-Mission::check_mission_valid()
+Mission::check_mission_valid(bool force)
 {
 
-	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+	if ((!_home_inited && _navigator->home_position_valid()) || force) {
 
-	_navigator->get_mission_result()->valid =
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
-		_missionFeasibilityChecker.checkMissionFeasible(
-			_navigator->get_mavlink_log_pub(),
-			(_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
-			dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-			_navigator->get_home_position()->alt,
-			_navigator->home_position_valid(),
-			_navigator->get_global_position()->lat,
-			_navigator->get_global_position()->lon,
-			_param_dist_1wp.get(),
-			_navigator->get_mission_result()->warning,
-			_navigator->get_default_acceptance_radius(),
-			_navigator->get_land_detected()->landed);
 
-	_navigator->increment_mission_instance_count();
+		_navigator->get_mission_result()->valid =
-	_navigator->set_mission_result_updated();
+			_missionFeasibilityChecker.checkMissionFeasible(
+				_navigator->get_mavlink_log_pub(),
+				(_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
+				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+				_navigator->get_home_position()->alt,
+				_navigator->home_position_valid(),
+				_navigator->get_global_position()->lat,
+				_navigator->get_global_position()->lon,
+				_param_dist_1wp.get(),
+				_navigator->get_mission_result()->warning,
+				_navigator->get_default_acceptance_radius(),
+				_navigator->get_land_detected()->landed);
+
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
+		_home_inited = _navigator->home_position_valid();
+	}
 }
 
 void
@@ -1177,7 +1222,7 @@ Mission::reset_offboard_mission(struct mission_s &mission)
 						item.do_jump_current_count = 0;
 
 						if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET,
-								 &item, len) != len) {
+							     &item, len) != len) {
 							PX4_WARN("could not save mission item during reset");
 							break;
 						}

src/modules/navigator/mission.h

@@ -211,7 +211,8 @@ private:
 	/**
 	 * Check wether a mission is ready to go
 	 */
-	void check_mission_valid();
+	void check_mission_valid(bool force);
+
 
 	/**
 	 * Reset offboard mission
@@ -244,6 +245,7 @@ private:
 	} _mission_type;
 
 	bool _inited;
+	bool _home_inited;
 	bool _need_mission_reset;
 
 	MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */