Mission feasibility: Geofence check fix

Current implementation checks against geofence only if it is in polygon form.
When it's created via params, it accepts all the waypoints as the number of vertices = 0.
Thus, changed the function to the one that is used to check whether geofence is breached in flight.

src/modules/navigator/geofence.cpp

@@ -126,6 +126,11 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
 	}
 }
 
+bool Geofence::inside(const struct mission_item_s &mission_item)
+{
+	return inside(mission_item.lat, mission_item.lon, mission_item.altitude);
+}
+
 bool Geofence::inside(double lat, double lon, float altitude)
 {
 		float max_horizontal_distance = _param_max_hor_distance.get();

src/modules/navigator/geofence.h

@@ -88,6 +88,8 @@ public:
 		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
 		    const struct home_position_s home_pos, bool home_position_set);
 
+	bool inside(const struct mission_item_s &mission_item);
+
 	bool inside_polygon(double lat, double lon, float altitude);
 
 	int clearDm();

src/modules/navigator/mission_feasibility_checker.cpp

@@ -87,7 +87,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p
 
 	// check if all mission item commands are supported
 	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed);
-	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence);
+	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt);
 	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
 
 	if (isRotarywing) {
@@ -149,7 +149,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
 	return resLanding;
 }
 
-bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
 	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
 	if (geofence.valid()) {
@@ -162,10 +162,15 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 				return false;
 			}
 
-			if (MissionBlock::item_contains_position(&missionitem) &&
-				!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
+			// Geofence function checks against home altitude amsl
+			missionitem.altitude = missionitem.altitude_is_relative
+				      ? missionitem.altitude + home_alt
+			              : missionitem.altitude;
 
-				mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i);
+			if (MissionBlock::item_contains_position(&missionitem) &&
+				!geofence.inside(missionitem)) {
+
+				mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1);
 				return false;
 			}
 		}

src/modules/navigator/mission_feasibility_checker.h

@@ -62,7 +62,7 @@ private:
 	void init();
 
 	/* Checks for all airframes */
-	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
 	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false);
 	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
 	bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued);