#include <geofence.h>

Inheritance diagram for Geofence:

Collaboration diagram for Geofence:

Classes
struct	PolygonInfo

Public Types
enum	{ GF_ALT_MODE_WGS84 = 0, GF_ALT_MODE_AMSL = 1 }

enum	{ GF_SOURCE_GLOBALPOS = 0, GF_SOURCE_GPS = 1 }

Public Member Functions
	Geofence (Navigator *navigator)

	Geofence (const Geofence &)=delete

Geofence &	operator= (const Geofence &)=delete

	~Geofence ()

void	updateFence ()
	update the geofence from dataman. More...

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. More...

bool	check (const struct mission_item_s &mission_item)
	Return whether a mission item obeys the geofence. More...

int	clearDm ()

bool	valid ()

int	loadFromFile (const char *filename)
	Load a single inclusion polygon, replacing any already existing polygons. More...

bool	isEmpty ()

int	getAltitudeMode ()

int	getSource ()

int	getGeofenceAction ()

bool	isHomeRequired ()

void	printStatus ()
	print Geofence status to the console More...

Private Member Functions
void	_updateFence ()
	implementation of updateFence(), but without locking More...

bool	checkPolygons (double lat, double lon, float altitude)
	Check if a point passes the Geofence test. More...

bool	checkAll (double lat, double lon, float altitude)
	Check if a point passes the Geofence test. More...

bool	checkAll (const vehicle_global_position_s &global_position)

bool	checkAll (const vehicle_global_position_s &global_position, float baro_altitude_amsl)

bool	insidePolygon (const PolygonInfo &polygon, double lat, double lon, float altitude)
	Check if a single point is within a polygon. More...

bool	insideCircle (const PolygonInfo &polygon, double lat, double lon, float altitude)
	Check if a single point is within a circle. More...

Private Attributes
Navigator *	_navigator {nullptr}

hrt_abstime	_last_horizontal_range_warning {0}

hrt_abstime	_last_vertical_range_warning {0}

float	_altitude_min {0.0f}

float	_altitude_max {0.0f}

PolygonInfo *	_polygons {nullptr}

int	_num_polygons {0}

map_projection_reference_s	_projection_reference = {}
	reference to convert (lon, lat) to local [m] More...

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 {0}

uint16_t	_update_counter {0}
	dataman update counter: if it does not match, we polygon data was updated More...

Detailed Description

Definition at line 59 of file geofence.h.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum

Enumerator
GF_ALT_MODE_WGS84
GF_ALT_MODE_AMSL

Definition at line 68 of file geofence.h.

◆ anonymous enum

anonymous enum

Enumerator
GF_SOURCE_GLOBALPOS
GF_SOURCE_GPS

Definition at line 74 of file geofence.h.

Constructor & Destructor Documentation

◆ Geofence() [1/2]

Geofence::Geofence ( Navigator * navigator )

Definition at line 55 of file geofence.cpp.

References _updateFence().

Here is the call graph for this function:

◆ Geofence() [2/2]

Geofence::Geofence ( const Geofence & )

delete

◆ ~Geofence()

Geofence::~Geofence ( )

Definition at line 64 of file geofence.cpp.

References _polygons.

Member Function Documentation

◆ _updateFence()

void Geofence::_updateFence ( )

private

implementation of updateFence(), but without locking

Definition at line 86 of file geofence.cpp.

References _num_polygons, _polygons, _update_counter, vehicle_global_position_s::alt, checkAll(), Geofence::PolygonInfo::circle_radius, mission_fence_point_s::circle_radius, Geofence::PolygonInfo::dataman_index, DM_KEY_FENCE_POINTS, dm_read(), Geofence::PolygonInfo::fence_type, vehicle_global_position_s::lat, vehicle_global_position_s::lon, mission_fence_point_s::nav_cmd, NAV_CMD_FENCE_CIRCLE_EXCLUSION, NAV_CMD_FENCE_CIRCLE_INCLUSION, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION, NAV_CMD_FENCE_RETURN_POINT, mission_stats_entry_s::num_items, mission_stats_entry_s::update_counter, Geofence::PolygonInfo::vertex_count, and mission_fence_point_s::vertex_count.

Referenced by checkPolygons(), Geofence(), and updateFence().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ check() [1/2]

bool Geofence::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.

Returns: true: system is obeying fence, false: system is violating fence

Definition at line 190 of file geofence.cpp.

References vehicle_gps_position_s::alt, checkAll(), getAltitudeMode(), getSource(), GF_ALT_MODE_WGS84, GF_SOURCE_GLOBALPOS, vehicle_gps_position_s::lat, and vehicle_gps_position_s::lon.

Referenced by MissionFeasibilityChecker::checkGeofence(), and Navigator::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ check() [2/2]

bool Geofence::check ( const struct mission_item_s & mission_item )

Return whether a mission item obeys the geofence.

Returns: true: system is obeying fence, false: system is violating fence

Definition at line 216 of file geofence.cpp.

References mission_item_s::altitude, checkAll(), mission_item_s::lat, and mission_item_s::lon.

Here is the call graph for this function:

◆ checkAll() [1/3]

bool Geofence::checkAll	(	double	lat,
		double	lon,
		float	altitude
	)

private

Check if a point passes the Geofence test.

In addition to checkPolygons(), this takes all additional parameters into account.

Returns: false for a geofence violation

Definition at line 221 of file geofence.cpp.

References _last_horizontal_range_warning, _last_vertical_range_warning, _navigator, _outside_counter, home_position_s::alt, checkPolygons(), FLT_EPSILON, GEOFENCE_RANGE_WARNING_LIMIT, get_distance_to_point_global_wgs84(), Navigator::get_home_position(), Navigator::get_mavlink_log_pub(), Navigator::home_position_valid(), hrt_absolute_time(), hrt_elapsed_time(), isHomeRequired(), home_position_s::lat, home_position_s::lon, and mavlink_log_critical.

Referenced by _updateFence(), and check().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkAll() [2/3]

bool Geofence::checkAll ( const vehicle_global_position_s & global_position )

private

◆ checkAll() [3/3]

bool Geofence::checkAll	(	const vehicle_global_position_s &	global_position,
		float	baro_altitude_amsl
	)

private

◆ checkPolygons()

bool Geofence::checkPolygons	(	double	lat,
		double	lon,
		float	altitude
	)

private

Check if a point passes the Geofence test.

This takes all polygons and minimum & maximum altitude into account

The check passes if: (inside(polygon_inclusion_1) || inside(polygon_inclusion_2) || ... ) && !inside(polygon_exclusion_1) && !inside(polygon_exclusion_2) && ... && (altitude within [min, max]) or: no polygon configured

Returns: result of the check above (false for a geofence violation)

Definition at line 281 of file geofence.cpp.

References _altitude_max, _altitude_min, _num_polygons, _polygons, _update_counter, _updateFence(), DM_KEY_FENCE_POINTS, dm_read(), dm_trylock(), dm_unlock(), insideCircle(), insidePolygon(), isEmpty(), NAV_CMD_FENCE_CIRCLE_EXCLUSION, NAV_CMD_FENCE_CIRCLE_INCLUSION, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION, and mission_stats_entry_s::update_counter.

Referenced by checkAll().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ clearDm()

int Geofence::clearDm ( )

Definition at line 562 of file geofence.cpp.

References dm_clear(), DM_KEY_FENCE_POINTS, and updateFence().

Referenced by loadFromFile().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getAltitudeMode()

int Geofence::getAltitudeMode ( )

inline

Definition at line 127 of file geofence.h.

Referenced by check().

Here is the caller graph for this function:

◆ getGeofenceAction()

int Geofence::getGeofenceAction ( )

inline

Definition at line 129 of file geofence.h.

References isHomeRequired(), and printStatus().

Referenced by isHomeRequired(), and Navigator::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getSource()

int Geofence::getSource ( )

inline

Definition at line 128 of file geofence.h.

Referenced by check(), and Navigator::run().

Here is the caller graph for this function:

◆ insideCircle()

bool Geofence::insideCircle	(	const PolygonInfo &	polygon,
		double	lat,
		double	lon,
		float	altitude
	)

private

Check if a single point is within a circle.

Parameters

polygon must be a circle!

Returns: true if within polygon the circle

Definition at line 399 of file geofence.cpp.

References _projection_reference, mission_fence_point_s::circle_radius, Geofence::PolygonInfo::dataman_index, DM_KEY_FENCE_POINTS, dm_read(), mission_fence_point_s::frame, mission_fence_point_s::lat, mission_fence_point_s::lon, map_projection_init(), map_projection_initialized(), map_projection_project(), NAV_FRAME_GLOBAL, NAV_FRAME_GLOBAL_INT, NAV_FRAME_GLOBAL_RELATIVE_ALT, and NAV_FRAME_GLOBAL_RELATIVE_ALT_INT.

Referenced by checkPolygons().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ insidePolygon()

bool Geofence::insidePolygon	(	const PolygonInfo &	polygon,
		double	lat,
		double	lon,
		float	altitude
	)

private

Check if a single point is within a polygon.

Returns: true if within polygon

Definition at line 357 of file geofence.cpp.

References Geofence::PolygonInfo::dataman_index, DM_KEY_FENCE_POINTS, dm_read(), mission_fence_point_s::frame, mission_fence_point_s::lat, mission_fence_point_s::lon, NAV_FRAME_GLOBAL, NAV_FRAME_GLOBAL_INT, NAV_FRAME_GLOBAL_RELATIVE_ALT, NAV_FRAME_GLOBAL_RELATIVE_ALT_INT, and Geofence::PolygonInfo::vertex_count.

Referenced by checkPolygons().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isEmpty()

bool Geofence::isEmpty ( )

inline

Definition at line 125 of file geofence.h.

References _num_polygons.

Referenced by checkPolygons().

Here is the caller graph for this function:

◆ isHomeRequired()

bool Geofence::isHomeRequired ( )

Definition at line 569 of file geofence.cpp.

References FLT_EPSILON, and getGeofenceAction().

Referenced by checkAll(), MissionFeasibilityChecker::checkGeofence(), getGeofenceAction(), and Navigator::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ loadFromFile()

int Geofence::loadFromFile ( const char * filename )

Load a single inclusion polygon, replacing any already existing polygons.

The file has one of the following formats:

Decimal Degrees: 0 900 47.475273548913222 8.52672100067138672 47.4608261578541359 8.53414535522460938 47.4613484218861217 8.56444358825683594 47.4830758091035534 8.53470325469970703
Degree-Minute-Second: 0 900 DMS -26 -34 -10.4304 151 50 14.5428 DMS -26 -34 -11.8416 151 50 21.8580 DMS -26 -34 -36.5628 151 50 28.1112 DMS -26 -34 -37.1640 151 50 24.1620

Where the first line is min, max altitude in meters AMSL.

Definition at line 436 of file geofence.cpp.

References _altitude_max, _altitude_min, _navigator, mission_fence_point_s::alt, clearDm(), DM_KEY_FENCE_POINTS, DM_PERSIST_POWER_ON_RESET, dm_read(), dm_write(), mission_fence_point_s::frame, GEOFENCE_FILENAME, Navigator::get_mavlink_log_pub(), mission_fence_point_s::lat, mission_fence_point_s::lon, mavlink_log_critical, mavlink_log_info, mission_fence_point_s::nav_cmd, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION, NAV_FRAME_GLOBAL, mission_stats_entry_s::num_items, updateFence(), and mission_fence_point_s::vertex_count.

Referenced by Navigator::load_fence_from_file(), and Navigator::run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ operator=()

Geofence& Geofence::operator= ( const Geofence & )

delete

◆ printStatus()

void Geofence::printStatus ( )

print Geofence status to the console

Definition at line 578 of file geofence.cpp.

References _num_polygons, _polygons, NAV_CMD_FENCE_CIRCLE_EXCLUSION, NAV_CMD_FENCE_CIRCLE_INCLUSION, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION, and Geofence::PolygonInfo::vertex_count.

Referenced by getGeofenceAction(), and Navigator::print_status().

Here is the caller graph for this function:

◆ updateFence()

void Geofence::updateFence ( )

update the geofence from dataman.

It's generally not necessary to call this as it will automatically update when the data is changed.

Definition at line 71 of file geofence.cpp.

References _updateFence(), DM_KEY_FENCE_POINTS, dm_lock(), and dm_unlock().

Referenced by clearDm(), and loadFromFile().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ valid()

bool Geofence::valid ( )

Definition at line 430 of file geofence.cpp.

Referenced by MissionFeasibilityChecker::checkGeofence().

Here is the caller graph for this function:

Member Data Documentation

◆ _altitude_max

float Geofence::_altitude_max {0.0f}

private

Definition at line 145 of file geofence.h.

Referenced by checkPolygons(), and loadFromFile().

◆ _altitude_min

float Geofence::_altitude_min {0.0f}

private

Definition at line 144 of file geofence.h.

Referenced by checkPolygons(), and loadFromFile().

◆ _last_horizontal_range_warning

hrt_abstime Geofence::_last_horizontal_range_warning {0}

private

Definition at line 141 of file geofence.h.

Referenced by checkAll().

◆ _last_vertical_range_warning

hrt_abstime Geofence::_last_vertical_range_warning {0}

private

Definition at line 142 of file geofence.h.

Referenced by checkAll().

◆ _navigator

Navigator* Geofence::_navigator {nullptr}

private

Definition at line 139 of file geofence.h.

Referenced by checkAll(), and loadFromFile().

◆ _num_polygons

int Geofence::_num_polygons {0}

private

Definition at line 156 of file geofence.h.

Referenced by _updateFence(), checkPolygons(), isEmpty(), and printStatus().

◆ _outside_counter

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 Geofence::_outside_counter {0}

private

Definition at line 171 of file geofence.h.

Referenced by checkAll().

◆ _polygons

PolygonInfo* Geofence::_polygons {nullptr}

private

Definition at line 155 of file geofence.h.

Referenced by _updateFence(), checkPolygons(), printStatus(), and ~Geofence().

◆ _projection_reference

map_projection_reference_s Geofence::_projection_reference = {}

private

reference to convert (lon, lat) to local [m]

Definition at line 158 of file geofence.h.

Referenced by insideCircle().

◆ _update_counter

uint16_t Geofence::_update_counter {0}

private

dataman update counter: if it does not match, we polygon data was updated

Definition at line 172 of file geofence.h.

Referenced by _updateFence(), and checkPolygons().

The documentation for this class was generated from the following files:

src/modules/navigator/geofence.h
src/modules/navigator/geofence.cpp

Classes

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ anonymous enum

◆ anonymous enum

Constructor & Destructor Documentation

◆ Geofence() [1/2]

◆ Geofence() [2/2]

◆ ~Geofence()

Member Function Documentation

◆ _updateFence()

◆ check() [1/2]

◆ check() [2/2]

◆ checkAll() [1/3]

◆ checkAll() [2/3]

◆ checkAll() [3/3]

◆ checkPolygons()

◆ clearDm()

◆ getAltitudeMode()

◆ getGeofenceAction()

◆ getSource()

◆ insideCircle()

◆ insidePolygon()

◆ isEmpty()

◆ isHomeRequired()

◆ loadFromFile()

◆ operator=()

◆ printStatus()

◆ updateFence()

◆ valid()

Member Data Documentation

◆ _altitude_max

◆ _altitude_min

◆ _last_horizontal_range_warning

◆ _last_vertical_range_warning

◆ _navigator

◆ _num_polygons

◆ _outside_counter

◆ _polygons

◆ _projection_reference

◆ _update_counter