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