diff options
Diffstat (limited to 'src/modules/navigator/geofence.h')
-rw-r--r-- | src/modules/navigator/geofence.h | 63 |
1 files changed, 48 insertions, 15 deletions
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 2eb126ab5..9d647cb68 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,6 +42,9 @@ #define GEOFENCE_H_ #include <uORB/topics/fence.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/sensor_combined.h> #include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> @@ -49,30 +52,32 @@ class Geofence : public control::SuperBlock { -private: - orb_advert_t _fence_pub; /**< publish fence topic */ - - float _altitude_min; - float _altitude_max; - - unsigned _verticesCount; - - /* Params */ - control::BlockParamInt param_geofence_on; public: Geofence(); ~Geofence(); + /* Altitude mode, corresponding to the param GF_ALTMODE */ + enum { + GF_ALT_MODE_WGS84 = 0, + GF_ALT_MODE_AMSL = 1 + }; + + /* Source, corresponding to the param GF_SOURCE */ + enum { + GF_SOURCE_GLOBALPOS = 0, + GF_SOURCE_GPS = 1 + }; + /** - * Return whether craft is inside geofence. + * Return whether system is inside geofence. * * Calculate whether point is inside arbitrary polygon * @param craft pointer craft coordinates - * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected - * @return true: craft is inside fence, false:craft is outside fence + * @return true: system is inside fence, false: system is outside fence */ - bool inside(const struct vehicle_global_position_s *craft); - bool inside(double lat, double lon, float altitude); + bool inside(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -88,6 +93,34 @@ public: int loadFromFile(const char *filename); bool isEmpty() {return _verticesCount == 0;} + + int getAltitudeMode() { return _param_altitude_mode.get(); } + + int getSource() { return _param_source.get(); } + + void setMavlinkFd(int value) { _mavlinkFd = value; } + +private: + orb_advert_t _fence_pub; /**< publish fence topic */ + + float _altitude_min; + float _altitude_max; + + unsigned _verticesCount; + + /* Params */ + control::BlockParamInt _param_geofence_on; + control::BlockParamInt _param_altitude_mode; + control::BlockParamInt _param_source; + control::BlockParamInt _param_counter_threshold; + + uint8_t _outside_counter; + + int _mavlinkFd; + + bool inside(double lat, double lon, float altitude); + bool inside(const struct vehicle_global_position_s &global_position); + bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); }; |