45 #include <px4_platform_common/module_params.h> 48 #include <px4_platform_common/defines.h> 55 #define GEOFENCE_FILENAME PX4_STORAGEDIR"/etc/geofence.txt" 161 (ParamInt<px4::params::GF_ACTION>) _param_gf_action,
162 (ParamInt<px4::params::GF_ALTMODE>) _param_gf_altmode,
163 (ParamInt<px4::params::GF_SOURCE>) _param_gf_source,
164 (ParamInt<px4::params::GF_COUNT>) _param_gf_count,
165 (ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_gf_max_hor_dist,
166 (ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist
197 bool checkAll(
double lat,
double lon,
float altitude);
void _updateFence()
implementation of updateFence(), but without locking
bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude)
Check if a single point is within a circle.
Global position setpoint in WGS84 coordinates.
Definition of geo / math functions to perform geodesic calculations.
hrt_abstime _last_vertical_range_warning
bool checkAll(double lat, double lon, float altitude)
Check if a point passes the Geofence test.
bool insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude)
Check if a single point is within a polygon.
hrt_abstime _last_horizontal_range_warning
uint16_t fence_type
one of MAV_CMD_NAV_FENCE_* (can also be a circular region)
void updateFence()
update the geofence from dataman.
void printStatus()
print Geofence status to the console
High-resolution timer with callouts and timekeeping.
Geofence(Navigator *navigator)
map_projection_reference_s _projection_reference
reference to convert (lon, lat) to local [m]
DEFINE_PARAMETERS((ParamInt< px4::params::GF_ACTION >) _param_gf_action,(ParamInt< px4::params::GF_ALTMODE >) _param_gf_altmode,(ParamInt< px4::params::GF_SOURCE >) _param_gf_source,(ParamInt< px4::params::GF_COUNT >) _param_gf_count,(ParamFloat< px4::params::GF_MAX_HOR_DIST >) _param_gf_max_hor_dist,(ParamFloat< px4::params::GF_MAX_VER_DIST >) _param_gf_max_ver_dist) uORB int _outside_counter
bool check(const vehicle_global_position_s &global_position, const vehicle_gps_position_s &gps_position, const home_position_s home_pos, bool home_position_set)
Return whether the system obeys the geofence.
bool checkPolygons(double lat, double lon, float altitude)
Check if a point passes the Geofence test.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Geofence & operator=(const Geofence &)=delete
int loadFromFile(const char *filename)
Load a single inclusion polygon, replacing any already existing polygons.
uint16_t _update_counter
dataman update counter: if it does not match, we polygon data was updated