mirror of https://github.com/ArduPilot/ardupilot
441 lines
16 KiB
C++
441 lines
16 KiB
C++
#pragma once
|
|
|
|
#include "AC_Fence_config.h"
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
// 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;
|
|
};
|
|
|
|
#if AP_FENCE_ENABLED
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Common/Location.h>
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
|
|
class AC_PolyFence_loader
|
|
{
|
|
|
|
public:
|
|
|
|
AC_PolyFence_loader(AP_Int8 &total, const AP_Int16 &options) :
|
|
_total(total),
|
|
_options(options) {}
|
|
|
|
/* Do not allow copies */
|
|
CLASS_NO_COPY(AC_PolyFence_loader);
|
|
|
|
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();
|
|
|
|
// 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;
|
|
|
|
// 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;
|
|
const AP_Int16 &_options;
|
|
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
|