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

geofence: implement circular areas

Browse Source
This commit is contained in:
Beat Küng
2017-08-04 18:08:51 +02:00
committed by Lorenz Meier
parent 6667b6434b
commit ed478f40fd
4 changed files with 143 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.h
+15 -2
View File
@@ -47,6 +47,7 @@
#include <controllib/block/BlockParam.hpp>
#include <controllib/blocks.hpp>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <px4_defines.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -145,13 +146,18 @@ private:
float _altitude_max{0.0f};
struct PolygonInfo {
uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_*
uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region)
uint16_t dataman_index;
uint16_t vertex_count;
union {
uint16_t vertex_count;
float circle_radius;
};
};
PolygonInfo *_polygons{nullptr};
int _num_polygons{0};
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
/* Params */
control::BlockParamInt _param_action;
control::BlockParamInt _param_altitude_mode;
@@ -190,6 +196,13 @@ private:
* @return true if within polygon
*/
bool insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude);
/**
* Check if a single point is within a circle
* @param polygon must be a circle!
* @return true if within polygon the circle
*/
bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude);
};
#endif /* GEOFENCE_H_ */