ardupilot/libraries/AC_Fence/AC_PolyFence_loader.h

443 lines
16 KiB
C++

#pragma once
#include "AC_Fence_config.h"
#if AP_FENCE_ENABLED
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#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 &center_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 &center_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);
};
#endif // AP_FENCE_ENABLED