fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 21:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator status: print how many polygons there are currently loaded

Browse Source
This commit is contained in:
Beat Küng
2017-08-04 18:08:51 +02:00
committed by Lorenz Meier
parent 4c1328483d
commit 401d6a1a6f
3 changed files with 27 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+20
View File
@@ -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);
}