From 401d6a1a6f67c8397d46fa34e74bf03d2c216ef1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 29 Mar 2017 12:01:23 +0200
Subject: [PATCH] navigator status: print how many polygons there are currently
 loaded

---
 src/modules/navigator/geofence.cpp       | 20 ++++++++++++++++++++
 src/modules/navigator/geofence.h         |  5 +++++
 src/modules/navigator/navigator_main.cpp |  2 ++
 3 files changed, 27 insertions(+)

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index b9367cadb1..6671ff7d82 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -481,3 +481,23 @@ bool Geofence::isHomeRequired()
 
 	return max_horizontal_enabled || max_vertical_enabled || geofence_action_rtl;
 }
+
+void Geofence::printStatus()
+{
+	int num_inclusion_polygons = 0, num_exclusion_polygons = 0, total_num_vertices = 0;
+
+	for (int i = 0; i < _num_polygons; ++i) {
+		total_num_vertices += _polygons[i].vertex_count;
+
+		if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) {
+			++num_inclusion_polygons;
+		}
+
+		if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
+			++num_exclusion_polygons;
+		}
+	}
+
+	PX4_INFO("Geofence: %i inclusion, %i exclusion polygons, %i total vertices",
+		 num_inclusion_polygons, num_exclusion_polygons, total_num_vertices);
+}
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 68cbc4eacd..3bebd69af9 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -134,6 +134,11 @@ public:
 
 	bool isHomeRequired();
 
+	/**
+	 * print Geofence status to the console
+	 */
+	void printStatus();
+
 private:
 	Navigator	*_navigator{nullptr};
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 5ec9c072b6..830cd29dd6 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -750,6 +750,8 @@ Navigator::status()
 	// 	PX4_INFO("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
 	// }
 
+	_geofence.printStatus();
+
 }
 
 void