navigator geofence: switch to new dataman data structure, support multiple polygons

This also removes the 'navigator fence <lon> <lat>' command to simplify
code (I don't think there's still use for that anymore). However the
file loading is still supported.

If goefence.txt does not exist, navigator will not clear the geofence
anymore on startup.

src/modules/navigator/geofence.h

@@ -67,7 +67,7 @@ public:
 	Geofence(Navigator *navigator);
 	Geofence(const Geofence &) = delete;
 	Geofence &operator=(const Geofence &) = delete;
-	~Geofence() = default;
+	~Geofence();
 
 	/* Altitude mode, corresponding to the param GF_ALTMODE */
 	enum {
@@ -82,31 +82,52 @@ public:
 	};
 
 	/**
-	 * Return whether system is inside geofence.
-	 *
-	 * Calculate whether point is inside arbitrary polygon
-	 * @param craft pointer craft coordinates
-	 * @return true: system is inside fence, false: system is outside fence
+	 * update the geofence from dataman
 	 */
-	bool inside(const struct vehicle_global_position_s &global_position,
-		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl);
+	void updateFence();
 
-	bool inside(const struct mission_item_s &mission_item);
+	/**
+	 * Return whether the system obeys the geofence.
+	 *
+	 * @return true: system is obeying fence, false: system is violating fence
+	 */
+	bool check(const struct vehicle_global_position_s &global_position,
+		   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_polygon(double lat, double lon, float altitude);
+	/**
+	 * Return whether a mission item obeys the geofence.
+	 *
+	 * @return true: system is obeying fence, false: system is violating fence
+	 */
+	bool check(const struct mission_item_s &mission_item);
 
 	int clearDm();
 
 	bool valid();
 
 	/**
-	 * Specify fence vertex position.
+	 * Load a single inclusion polygon, replacing any already existing polygons.
+	 * The file has one of the following formats:
+	 * - Decimal Degrees:
+	 * 0 900
+	 * 47.475273548913222 8.52672100067138672
+	 * 47.4608261578541359 8.53414535522460938
+	 * 47.4613484218861217 8.56444358825683594
+	 * 47.4830758091035534 8.53470325469970703
+	 *
+	 * - Degree-Minute-Second:
+	 * 0 900
+	 * DMS -26 -34 -10.4304 151 50 14.5428
+	 * DMS -26 -34 -11.8416 151 50 21.8580
+	 * DMS -26 -34 -36.5628 151 50 28.1112
+	 * DMS -26 -34 -37.1640 151 50 24.1620
+	 *
+	 * Where the first line is min, max altitude in meters AMSL.
 	 */
-	void addPoint(int argc, char *argv[]);
-
 	int loadFromFile(const char *filename);
 
-	bool isEmpty() {return _vertices_count == 0;}
+	bool isEmpty() { return _num_polygons == 0; }
 
 	int getAltitudeMode() { return _param_altitude_mode.get(); }
 	int getSource() { return _param_source.get(); }
@@ -126,7 +147,13 @@ private:
 	float _altitude_min{0.0f};
 	float _altitude_max{0.0f};
 
-	unsigned _vertices_count{0};
+	struct PolygonInfo {
+		uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_*
+		uint16_t dataman_index;
+		uint16_t vertex_count;
+	};
+	PolygonInfo *_polygons{nullptr};
+	int _num_polygons{0};
 
 	/* Params */
 	control::BlockParamInt _param_action;
@@ -138,9 +165,34 @@ private:
 
 	int _outside_counter{0};
 
-	bool inside(double lat, double lon, float altitude);
-	bool inside(const struct vehicle_global_position_s &global_position);
-	bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
+	/**
+	 * Check if a point passes the Geofence test.
+	 * This takes all polygons and minimum & maximum altitude into account
+	 *
+	 * The check passes if: (inside(polygon_inclusion_1) || inside(polygon_inclusion_2) || ... ) &&
+	 *                       !inside(polygon_exclusion_1) && !inside(polygon_exclusion_2) && ...
+	 *                       && (altitude within [min, max])
+	 *                  or: no polygon configured
+	 * @return result of the check above (false for a geofence violation)
+	 */
+	bool checkPolygons(double lat, double lon, float altitude);
+
+	/**
+	 * Check if a point passes the Geofence test.
+	 * In addition to checkPolygons(), this takes all additional parameters into account.
+	 *
+	 * @return false for a geofence violation
+	 */
+	bool checkAll(double lat, double lon, float altitude);
+
+	bool checkAll(const struct vehicle_global_position_s &global_position);
+	bool checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
+
+	/**
+	 * Check if a single point is within a polygon
+	 * @return true if within polygon
+	 */
+	bool insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude);
 };
 
 #endif /* GEOFENCE_H_ */