#pragma once #include #include #include #include #define AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT 1 // CIRCLE_INCLUSION_INT stores the radius an a 32-bit integer in // metres. This was a bug, and CIRCLE_INCLUSION was created to store // as a 32-bit float instead. We save as _INT in the case that the // radius looks like an integer as a backwards-compatability measure. // For 4.2 we might consider only loading _INT and always saving as // float, and in 4.3 considering _INT invalid enum class AC_PolyFenceType { END_OF_STORAGE = 99, POLYGON_INCLUSION = 98, POLYGON_EXCLUSION = 97, CIRCLE_EXCLUSION_INT = 96, RETURN_POINT = 95, CIRCLE_INCLUSION_INT = 94, CIRCLE_EXCLUSION = 93, CIRCLE_INCLUSION = 92, }; // a FenceItem is just a means of passing data about an item into // and out of the polyfence loader. It uses a AC_PolyFenceType to // indicate the item type, assuming each fence type is made up of // only one sort of item. // TODO: make this a union (or use subclasses) to save memory class AC_PolyFenceItem { public: AC_PolyFenceType type; Vector2l loc; uint8_t vertex_count; float radius; }; class AC_PolyFence_loader { public: AC_PolyFence_loader(AP_Int8 &total) : _total(total) {} AC_PolyFence_loader(const AC_PolyFence_loader &other) = delete; AC_PolyFence_loader &operator=(const AC_PolyFence_loader&) = delete; void init(); // methods primarily for MissionItemProtocol_Fence to use: // return the total number of points stored uint16_t num_stored_items() const { return _eeprom_item_count; } bool get_item(const uint16_t seq, AC_PolyFenceItem &item) WARN_IF_UNUSED; /// /// exclusion polygons /// /// returns number of polygon exclusion zones defined uint8_t get_exclusion_polygon_count() const { return _num_loaded_exclusion_boundaries; } /// returns pointer to array of exclusion polygon points and num_points is filled in with the number of points in the polygon /// points are offsets in cm from EKF origin in NE frame Vector2f* get_exclusion_polygon(uint16_t index, uint16_t &num_points) const; /// return system time of last update to the exclusion polygon points uint32_t get_exclusion_polygon_update_ms() const { return _load_time_ms; } /// /// inclusion polygons /// /// returns number of polygon inclusion zones defined uint8_t get_inclusion_polygon_count() const { return _num_loaded_inclusion_boundaries; } /// returns pointer to array of inclusion polygon points and num_points is filled in with the number of points in the polygon /// points are offsets in cm from EKF origin in NE frame Vector2f* get_inclusion_polygon(uint16_t index, uint16_t &num_points) const; /// return system time of last update to the inclusion polygon points uint32_t get_inclusion_polygon_update_ms() const { return _load_time_ms; } /// /// exclusion circles /// /// returns number of exclusion circles defined uint8_t get_exclusion_circle_count() const { return _num_loaded_circle_exclusion_boundaries; } /// returns the specified exclusion circle /// center is offsets in cm from EKF origin in NE frame, radius is in meters bool get_exclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const; /// return system time of last update to the exclusion circles uint32_t get_exclusion_circle_update_ms() const { return _load_time_ms; } /// /// inclusion circles /// /// returns number of inclusion circles defined uint8_t get_inclusion_circle_count() const { return _num_loaded_circle_inclusion_boundaries; } /// returns the specified inclusion circle /// center is offsets in cm from EKF origin in NE frame, radius is in meters bool get_inclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const; // false if margin < fence radius bool check_inclusion_circle_margin(float margin) const; /// /// mavlink /// /// handler for polygon fence messages with GCS void handle_msg(class GCS_MAVLINK &link, const mavlink_message_t& msg); // breached() - returns true if the vehicle has breached any fence bool breached() const WARN_IF_UNUSED; // breached(Location&) - returns true if location is outside the boundary bool breached(const Location& loc) const WARN_IF_UNUSED; // returns true if a polygonal include fence could be returned bool inclusion_boundary_available() const WARN_IF_UNUSED { return _num_loaded_inclusion_boundaries != 0; } // loaded - returns true if the fences have been loaded from // storage and are available for use bool loaded() const WARN_IF_UNUSED { return _load_time_ms != 0; }; // maximum number of fence points we can store in eeprom uint16_t max_items() const; // write_fence - validate and write count new_items to permanent storage bool write_fence(const AC_PolyFenceItem *new_items, uint16_t count) WARN_IF_UNUSED; /* * Loaded Fence functionality * * methods and members to do with fences stored in memory. The * locations are translated into offset-from-origin-in-metres */ // load polygon points stored in eeprom into // _loaded_offsets_from_origin and perform validation. returns // true if load successfully completed bool load_from_eeprom() WARN_IF_UNUSED; // allow threads to lock against AHRS update HAL_Semaphore &get_loaded_fence_semaphore(void) { return _loaded_fence_sem; } // call @10Hz to check for fence load being valid void update(); #if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT // get_return_point - returns latitude/longitude of return point. // This works with storage - the returned vector is absolute // lat/lon. bool get_return_point(Vector2l &ret) WARN_IF_UNUSED; #endif // return total number of fences - polygons and circles uint16_t total_fence_count() const { return (get_exclusion_polygon_count() + get_inclusion_polygon_count() + get_exclusion_circle_count() + get_inclusion_circle_count()); } private: // multi-thread access support HAL_Semaphore _loaded_fence_sem; /* * Fence storage Index related functions */ // FenceIndex - a class used to store information about a fence in // fence storage. class FenceIndex { public: AC_PolyFenceType type; uint16_t count; uint16_t storage_offset; }; // index_fence_count - returns the number of fences of type // currently in the index uint16_t index_fence_count(const AC_PolyFenceType type); // void_index - free resources for the index, forcing a reindex // (typically via check_indexed) void void_index() { delete[] _index; _index = nullptr; _index_attempted = false; _indexed = false; } // check_indexed - read eeprom and create index if the index does // not already exist bool check_indexed() WARN_IF_UNUSED; // find_first_fence - return first fence in index of specific type FenceIndex *find_first_fence(const AC_PolyFenceType type) const; // find_index_for_seq - returns true if seq is contained within a // fence. If it is, entry will be the relevant FenceIndex. i // will be the offset within _loaded_offsets_from_origin where the // first point in the fence is found bool find_index_for_seq(const uint16_t seq, const FenceIndex *&entry, uint16_t &i) const WARN_IF_UNUSED; // find_storage_offset_for_seq - uses the index to return an // offset into storage for an item bool find_storage_offset_for_seq(const uint16_t seq, uint16_t &offset, AC_PolyFenceType &type, uint16_t &vertex_count_offset) const WARN_IF_UNUSED; uint16_t sum_of_polygon_point_counts_and_returnpoint(); /* * storage-related methods - dealing with fence_storage */ // new_fence_storage_magic - magic number indicating fence storage // has been formatted for use by polygon fence storage code. // FIXME: ensure this is out-of-band for old lat/lon point storage static const uint8_t new_fence_storage_magic = 235; // validate_fence - returns true if new_items look completely valid bool validate_fence(const AC_PolyFenceItem *new_items, uint16_t count) const WARN_IF_UNUSED; // _eos_offset - stores the offset in storage of the // end-of-storage marker. Used by low-level manipulation code to // extend storage uint16_t _eos_offset; // formatted - returns true if the fence storage space seems to be // formatted for new-style fence storage bool formatted() const WARN_IF_UNUSED; // format - format the storage space for use by // the new polyfence code bool format() WARN_IF_UNUSED; /* * Loaded Fence functionality * * methods and members to do with fences stored in memory. The * locations are translated into offset-from-origin-in-metres */ // remove resources dedicated to the transformed fences - for // example, in _loaded_offsets_from_origin void unload(); // pointer into _loaded_offsets_from_origin where the return point // can be found: Vector2f *_loaded_return_point; // pointer into _loaded_points_lla where the return point // can be found: Vector2l *_loaded_return_point_lla; class InclusionBoundary { public: Vector2f *points; // pointer into the _loaded_offsets_from_origin array Vector2l *points_lla; // pointer into the _loaded_points_lla array uint8_t count; // count of points in the boundary }; InclusionBoundary *_loaded_inclusion_boundary; uint8_t _num_loaded_inclusion_boundaries; class ExclusionBoundary { public: Vector2f *points; // pointer into the _loaded_offsets_from_origin array Vector2l *points_lla; // pointer into the _loaded_points_lla_lla array uint8_t count; // count of points in the boundary }; ExclusionBoundary *_loaded_exclusion_boundary; uint8_t _num_loaded_exclusion_boundaries; // _loaded_offsets_from_origin - stores x/y offset-from-origin // coordinate pairs. Various items store their locations in this // allocation - the polygon boundaries and the return point, for // example. Vector2f *_loaded_offsets_from_origin; Vector2l *_loaded_points_lla; class ExclusionCircle { public: Vector2f pos_cm; // vector offset from home in cm Vector2l point; // lat/lng of zone float radius; }; ExclusionCircle *_loaded_circle_exclusion_boundary; uint8_t _num_loaded_circle_exclusion_boundaries; class InclusionCircle { public: Vector2f pos_cm; // vector offset from home in cm Vector2l point; // lat/lng of zone float radius; }; InclusionCircle *_loaded_circle_inclusion_boundary; uint8_t _num_loaded_circle_inclusion_boundaries; // _load_attempted - true if we have attempted to load the fences // from storage into _loaded_circle_exclusion_boundary, // _loaded_offsets_from_origin etc etc bool _load_attempted; // _load_time_ms - from millis(), system time when fence load last // succeeded. Will be zero if fences are not loaded uint32_t _load_time_ms; // scale_latlon_from_origin - given a latitude/longitude // transforms the point to an offset-from-origin and deposits // the result into pos_cm. bool scale_latlon_from_origin(const Location &origin, const Vector2l &point, Vector2f &pos_cm) WARN_IF_UNUSED; // read_polygon_from_storage - reads vertex_count // latitude/longitude points from offset in permanent storage, // transforms them into an offset-from-origin and deposits the // results into next_storage_point. bool read_polygon_from_storage(const Location &origin, uint16_t &read_offset, const uint8_t vertex_count, Vector2f *&next_storage_point, Vector2l *&next_storage_point_lla) WARN_IF_UNUSED; /* * Upgrade functions - attempt to keep user's fences when * upgrading to new firmware */ // convert_to_new_storage - will attempt to change a pre-existing // stored fence to the new storage format (so people don't lose // their fences when upgrading) bool convert_to_new_storage() WARN_IF_UNUSED; // load boundary point from eeprom, returns true on successful load bool load_point_from_eeprom(uint16_t i, Vector2l& point) const WARN_IF_UNUSED; #if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT /* * FENCE_POINT protocol compatability */ void handle_msg_fetch_fence_point(GCS_MAVLINK &link, const mavlink_message_t& msg); void handle_msg_fence_point(GCS_MAVLINK &link, const mavlink_message_t& msg); // contains_compatible_fence - returns true if the permanent fence // storage contains fences that are compatible with the old // FENCE_POINT protocol. bool contains_compatible_fence() const WARN_IF_UNUSED; // get_or_create_include_fence - returns a point to an include // fence to be used for the FENCE_POINT-supplied polygon. May // format the storage appropriately. FenceIndex *get_or_create_include_fence(); // get_or_create_include_fence - returns a point to a return point // to be used for the FENCE_POINT-supplied return point. May // format the storage appropriately. FenceIndex *get_or_create_return_point(); #endif // primitives to write parts of fencepoints out: bool write_type_to_storage(uint16_t &offset, AC_PolyFenceType type) WARN_IF_UNUSED; bool write_latlon_to_storage(uint16_t &offset, const Vector2l &latlon) WARN_IF_UNUSED; bool read_latlon_from_storage(uint16_t &read_offset, Vector2l &latlon) const WARN_IF_UNUSED; // methods to write specific types of fencepoint out: bool write_eos_to_storage(uint16_t &offset); // _total - reference to FENCE_TOTAL parameter. This is used // solely for compatability with the FENCE_POINT protocol AP_Int8 &_total; uint8_t _old_total; // scan_eeprom - a method that traverses the fence storage area, // calling the supplied callback for each fence found. If the // scan fails (for example, the storage is corrupt), then this // method will return false. FUNCTOR_TYPEDEF(scan_fn_t, void, const AC_PolyFenceType, uint16_t); bool scan_eeprom(scan_fn_t scan_fn) WARN_IF_UNUSED; // scan_eeprom_count_fences - a static function designed to be // massed to scan_eeprom which counts the number of fences and // fence items present. The results of this counting appear in _eeprom_fence_count and _eeprom_item_count void scan_eeprom_count_fences(const AC_PolyFenceType type, uint16_t read_offset); uint16_t _eeprom_fence_count; uint16_t _eeprom_item_count; // scan_eeprom_index_fences - a static function designed to be // passed to scan_eeprom. _index must be a pointer to // memory sufficient to hold information about all fences present // in storage - so it is expected that scan_eeprom_count_fences // has been used to count those fences and the allocation already // made. After this method has been called _index will // be filled with information about the fences in the fence // storage - type, item counts and storage offset. void scan_eeprom_index_fences(const AC_PolyFenceType type, uint16_t read_offset); // array specifying type of each fence in storage (and a count of // items in that fence) FenceIndex *_index; bool _indexed; // true if indexing successful bool _index_attempted; // true if we attempted to index the eeprom // _num_fences - count of the number of fences in _index. This // should be equal to _eeprom_fence_count uint16_t _num_fences; // count_eeprom_fences - refresh the count of fences in permanent storage bool count_eeprom_fences() WARN_IF_UNUSED; // index_eeprom - (re)allocate and fill in _index bool index_eeprom() WARN_IF_UNUSED; uint16_t fence_storage_space_required(const AC_PolyFenceItem *new_items, uint16_t count); };