diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 3e2a58f250..ec39ddd1b9 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -91,6 +91,7 @@ AC_Fence::AC_Fence() AP_Param::setup_object_defaults(this, var_info); } +/// enable the Fence code generally; a master switch for all fences void AC_Fence::enable(bool value) { _enabled = value; @@ -116,8 +117,8 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const return true; } - if (!_poly_loader.valid()) { - fail_msg = "Polygon boundary invalid"; + if (! _poly_loader.loaded()) { + fail_msg = "Fences invalid"; return false; } @@ -155,12 +156,6 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const return true; } - // check no limits are currently breached - if (_breached_fences) { - fail_msg = "vehicle outside fence"; - return false; - } - // if we have horizontal limits enabled, check we can get a // relative position from the AHRS if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) || @@ -184,10 +179,19 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const return false; } + // check no limits are currently breached + if (_breached_fences) { + fail_msg = "vehicle outside fence"; + return false; + } + // if we got this far everything must be ok return true; } +/// returns true if we have freshly breached the maximum altitude +/// fence; also may set up a fallback fence which, if breached, will +/// cause the altitude fence to be freshly breached bool AC_Fence::check_fence_alt_max() { // altitude fence check @@ -233,11 +237,14 @@ bool AC_Fence::check_fence_alt_max() return false; } -// check_fence_polygon - returns true if the polygon fence is freshly breached +// check_fence_polygon - returns true if the poly fence is freshly +// breached. That includes being inside exclusion zones and outside +// inclusions zones bool AC_Fence::check_fence_polygon() { const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON; - const bool breached = polygon_fence_is_breached(); + const bool breached = ((_enabled_fences & AC_FENCE_TYPE_POLYGON) && + _poly_loader.breached()); if (breached) { if (!was_breached) { record_breach(AC_FENCE_TYPE_POLYGON); @@ -251,17 +258,10 @@ bool AC_Fence::check_fence_polygon() return false; } -bool AC_Fence::polygon_fence_is_breached() -{ - if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) { - // not enabled; no breach - return false; - } - - return _poly_loader.breached(); -} - - +/// check_fence_circle - returns true if the circle fence (defined via +/// parameters) has been freshly breached. May also set up a backup +/// fence outside the fence and return a fresh breach if that backup +/// fence is breaced. bool AC_Fence::check_fence_circle() { if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) { @@ -426,118 +426,6 @@ void AC_Fence::manual_recovery_start() _manual_recovery_start_ms = AP_HAL::millis(); } -/// returns pointer to array of polygon points and num_points is filled in with the total number -Vector2f* AC_PolyFence_loader::get_boundary_points(uint16_t& num_points) const -{ - // return array minus the first point which holds the return location - if (_boundary == nullptr) { - return nullptr; - } - if (!valid()) { - return nullptr; - } - // minus one for return point, minus one for closing point - // (polygon().valid() is not true unless we have a closing point AND - // we have a minumum number of points) - if (_boundary_num_points < 2) { - return nullptr; - } - num_points = _boundary_num_points - 2; - return &_boundary[1]; -} - -/// handler for polygon fence messages with GCS -void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg) -{ - switch (msg.msgid) { - // receive a fence point from GCS and store in EEPROM - case MAVLINK_MSG_ID_FENCE_POINT: { - mavlink_fence_point_t packet; - mavlink_msg_fence_point_decode(&msg, &packet); - if (!check_latlng(packet.lat,packet.lng)) { - link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, lat or lng too large"); - } else { - Vector2l point; - point.x = packet.lat*1.0e7f; - point.y = packet.lng*1.0e7f; - if (!save_point_to_eeprom(packet.idx, point)) { - link.send_text(MAV_SEVERITY_WARNING, "Failed to save polygon point, too many points?"); - } else { - // trigger reload of points - _boundary_num_points = 0; - } - } - break; - } - - // send a fence point to GCS - case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { - mavlink_fence_fetch_point_t packet; - mavlink_msg_fence_fetch_point_decode(&msg, &packet); - // attempt to retrieve from eeprom - Vector2l point; - if (load_point_from_eeprom(packet.idx, point)) { - mavlink_msg_fence_point_send(link.get_chan(), msg.sysid, msg.compid, packet.idx, _total, point.x*1.0e-7f, point.y*1.0e-7f); - } else { - link.send_text(MAV_SEVERITY_WARNING, "Bad fence point"); - } - break; - } - - default: - // do nothing - break; - } -} - -/// load polygon points stored in eeprom into boundary array and perform validation -bool AC_PolyFence_loader::load_from_eeprom() -{ - // check if we need to create array - if (!_create_attempted) { - _boundary = (Vector2f *)create_point_array(sizeof(Vector2f)); - _create_attempted = true; - } - - // exit if we could not allocate RAM for the boundary - if (_boundary == nullptr) { - return false; - } - - // get current location from EKF - Location temp_loc; - if (!AP::ahrs_navekf().get_location(temp_loc)) { - return false; - } - struct Location ekf_origin {}; - if (!AP::ahrs().get_origin(ekf_origin)) { - return false; - } - - // sanity check total - _total = constrain_int16(_total, 0, max_points()); - - // load each point from eeprom - Vector2l temp_latlon; - for (uint16_t index=0; index<_total; index++) { - // load boundary point as lat/lon point - if (!load_point_from_eeprom(index, temp_latlon)) { - return false; - } - // move into location structure and convert to offset from ekf origin - temp_loc.lat = temp_latlon.x; - temp_loc.lng = temp_latlon.y; - _boundary[index] = ekf_origin.get_distance_NE(temp_loc) * 100.0f; - } - _boundary_num_points = _total; - _update_ms = AP_HAL::millis(); - - // update validity of polygon - _valid = calculate_boundary_valid(); - - return true; -} - // methods for mavlink SYS_STATUS message (send_sys_status) bool AC_Fence::sys_status_present() const { @@ -565,7 +453,7 @@ bool AC_Fence::sys_status_failed() const return true; } if (_enabled_fences & AC_FENCE_TYPE_POLYGON) { - if (!_poly_loader.valid()) { + if (!_poly_loader.inclusion_boundary_available()) { return true; } } @@ -591,6 +479,13 @@ bool AC_Fence::sys_status_failed() const return false; } +AC_PolyFence_loader &AC_Fence::polyfence() { + return _poly_loader; +} +const AC_PolyFence_loader &AC_Fence::polyfence() const { + return _poly_loader; +} + // singleton instance AC_Fence *AC_Fence::_singleton; diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 2f2e7ebbcd..83a048b1d0 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -41,6 +41,13 @@ public: AC_Fence(const AC_Fence &other) = delete; AC_Fence &operator=(const AC_Fence&) = delete; + void init() { + _poly_loader.init(); + } + + // get singleton instance + static AC_Fence *get_singleton() { return _singleton; } + /// enable - allows fence to be enabled/disabled. Note: this does not update the eeprom saved value void enable(bool value); @@ -50,6 +57,11 @@ public: /// get_enabled_fences - returns bitmask of enabled fences uint8_t get_enabled_fences() const; + // should be called @10Hz to handle loading from eeprom + void update() { + _poly_loader.update(); + } + /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully bool pre_arm_check(const char* &fail_msg) const; @@ -96,18 +108,15 @@ public: /// has no effect if no breaches have occurred void manual_recovery_start(); - static const struct AP_Param::GroupInfo var_info[]; - // methods for mavlink SYS_STATUS message (send_sys_status) bool sys_status_present() const; bool sys_status_enabled() const; bool sys_status_failed() const; - // get singleton instance - static AC_Fence *get_singleton() { return _singleton; } + AC_PolyFence_loader &polyfence(); + const AC_PolyFence_loader &polyfence() const; - AC_PolyFence_loader &polyfence() { return _poly_loader; } - const AC_PolyFence_loader &polyfence_const() const { return _poly_loader; } + static const struct AP_Param::GroupInfo var_info[]; private: static AC_Fence *_singleton; @@ -132,9 +141,6 @@ private: bool pre_arm_check_circle(const char* &fail_msg) const; bool pre_arm_check_alt(const char* &fail_msg) const; - // returns true if we have breached the fence: - bool polygon_fence_is_breached(); - // parameters AP_Int8 _enabled; // top level enable/disable control AP_Int8 _enabled_fences; // bit mask holding which fences are enabled @@ -166,7 +172,6 @@ private: uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control AC_PolyFence_loader _poly_loader{_total}; // polygon fence - }; namespace AP { diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index 2e7981d920..8363ab7612 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -1,35 +1,175 @@ #include "AC_PolyFence_loader.h" +#include +#include + +#include + +#define POLYFENCE_LOADER_DEBUGGING 1 + +#if POLYFENCE_LOADER_DEBUGGING +#define Debug(fmt, args ...) do { gcs().send_text(MAV_SEVERITY_INFO, fmt, ## args); } while (0) +#else +#define Debug(fmt, args ...) +#endif + extern const AP_HAL::HAL& hal; static const StorageAccess fence_storage(StorageManager::StorageFence); -/* - maximum number of fencepoints - */ -uint8_t AC_PolyFence_loader::max_points() const +void AC_PolyFence_loader::init() { - return MIN(255U, fence_storage.size() / sizeof(Vector2l)); + if (!check_indexed()) { + // tell the user, perhaps? + } + _old_total = _total; } -// create buffer to hold copy of eeprom points in RAM -// returns nullptr if not enough memory can be allocated -void* AC_PolyFence_loader::create_point_array(uint8_t element_size) +bool AC_PolyFence_loader::find_index_for_seq(const uint16_t seq, const FenceIndex *&entry, uint16_t &i) const { - uint32_t array_size = max_points() * element_size; - if (hal.util->available_memory() < 100U + array_size) { - // too risky to enable as we could run out of stack - return nullptr; + if (_index == nullptr) { + return false; } - return calloc(1, array_size); + if (seq > _eeprom_item_count) { + return false; + } + + i = 0; + for (uint16_t j=0; j<_num_fences; j++) { + entry = &_index[j]; + if (seq < i + entry->count) { + return true; + } + i += entry->count; + } + return false; +} + +bool AC_PolyFence_loader::find_storage_offset_for_seq(const uint16_t seq, uint16_t &offset, AC_PolyFenceType &type, uint16_t &vertex_count_offset) const +{ + if (_index == nullptr) { + return false; + } + + uint16_t i = 0; + const FenceIndex *entry = nullptr; + if (!find_index_for_seq(seq, entry, i)) { + return false; + } + + if (entry == nullptr) { + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return false; + } + + const uint16_t delta = seq - i; + + offset = entry->storage_offset; + type = entry->type; + offset++; // skip over type + switch (type) { + case AC_PolyFenceType::CIRCLE_INCLUSION: + case AC_PolyFenceType::CIRCLE_EXCLUSION: + if (delta != 0) { + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return false; + } + break; + case AC_PolyFenceType::POLYGON_INCLUSION: + case AC_PolyFenceType::POLYGON_EXCLUSION: + vertex_count_offset = offset; + offset += 1; // the count of points in the fence + offset += (delta * 8); + break; + case AC_PolyFenceType::RETURN_POINT: + if (delta != 0) { + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return false; + } + break; + case AC_PolyFenceType::END_OF_STORAGE: + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return false; + } + return true; +} + +bool AC_PolyFence_loader::get_item(const uint16_t seq, AC_PolyFenceItem &item) +{ + if (!check_indexed()) { + return false; + } + + uint16_t vertex_count_offset = 0; // initialised to make compiler happy + uint16_t offset; + AC_PolyFenceType type; + if (!find_storage_offset_for_seq(seq, offset, type, vertex_count_offset)) { + return false; + } + + item.type = type; + + switch (type) { + case AC_PolyFenceType::CIRCLE_INCLUSION: + case AC_PolyFenceType::CIRCLE_EXCLUSION: + if (!read_latlon_from_storage(offset, item.loc)) { + return false; + } + item.radius = fence_storage.read_uint32(offset); + offset += 4; + break; + case AC_PolyFenceType::POLYGON_INCLUSION: + case AC_PolyFenceType::POLYGON_EXCLUSION: + if (!read_latlon_from_storage(offset, item.loc)) { + return false; + } + item.vertex_count = fence_storage.read_uint8(vertex_count_offset); + break; + case AC_PolyFenceType::RETURN_POINT: + if (!read_latlon_from_storage(offset, item.loc)) { + return false; + } + break; + case AC_PolyFenceType::END_OF_STORAGE: + // read end-of-storage when I should never do so + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return false; + } + return true; +} + +bool AC_PolyFence_loader::write_type_to_storage(uint16_t &offset, const AC_PolyFenceType type) +{ + fence_storage.write_uint8(offset, (uint8_t)type); + offset++; + return true; +} + +bool AC_PolyFence_loader::write_latlon_to_storage(uint16_t &offset, const Vector2l &latlon) +{ + fence_storage.write_uint32(offset, latlon.x); + offset += 4; + fence_storage.write_uint32(offset, latlon.y); + offset += 4; + return true; +} + +bool AC_PolyFence_loader::read_latlon_from_storage(uint16_t &read_offset, Vector2l &ret) const +{ + ret.x = fence_storage.read_uint32(read_offset); + read_offset += 4; + ret.y = fence_storage.read_uint32(read_offset); + read_offset += 4; + return true; } // load boundary point from eeprom, returns true on successful load +// only used for converting from old storage to new storage bool AC_PolyFence_loader::load_point_from_eeprom(uint16_t i, Vector2l& point) { // sanity check index - if (i >= max_points()) { + if (i >= max_items()) { return false; } @@ -39,51 +179,7 @@ bool AC_PolyFence_loader::load_point_from_eeprom(uint16_t i, Vector2l& point) return true; } -// save a fence point to eeprom, returns true on successful save -bool AC_PolyFence_loader::save_point_to_eeprom(uint16_t i, const Vector2l& point) -{ - // sanity check index - if (i >= max_points()) { - return false; - } - - // write point to eeprom - fence_storage.write_uint32(i * sizeof(Vector2l), point.x); - fence_storage.write_uint32(i * sizeof(Vector2l)+sizeof(uint32_t), point.y); - return true; -} - -// validate array of boundary points -// returns true if boundary is valid -bool AC_PolyFence_loader::calculate_boundary_valid() const -{ - // exit immediate if no points - if (_boundary == nullptr) { - return false; - } - - // start from 2nd point as boundary contains return point (as first point) - uint8_t start_num = 1; - - // a boundary requires at least 4 point (a triangle and last point equals first) - if (_boundary_num_points < start_num + 4) { - return false; - } - - // point 1 and last point must be the same. Note: 0th point is reserved as the return point - if (!Polygon_complete(&_boundary[start_num], _boundary_num_points-start_num)) { - return false; - } - - // check return point is within the fence - if (Polygon_outside(_boundary[0], &_boundary[1], _boundary_num_points-start_num)) { - return false; - } - - return true; -} - -bool AC_PolyFence_loader::breached() +bool AC_PolyFence_loader::breached() const { // check if vehicle is outside the polygon fence Vector2f position; @@ -96,7 +192,7 @@ bool AC_PolyFence_loader::breached() return breached(position); } -bool AC_PolyFence_loader::breached(const Location& loc) +bool AC_PolyFence_loader::breached(const Location& loc) const { Vector2f posNE; if (!loc.get_vector_xy_from_origin_NE(posNE)) { @@ -106,27 +202,1341 @@ bool AC_PolyFence_loader::breached(const Location& loc) return breached(posNE); } +// check if a position (expressed as offsets in cm from the EKF origin) is within the boundary // returns true if location is outside the boundary -bool AC_PolyFence_loader::breached(const Vector2f& location) +bool AC_PolyFence_loader::breached(const Vector2f& pos_cm) const { - // check consistency of number of points - if (_boundary_num_points != _total) { - // Fence is currently not completely loaded. Can't breach it?! - load_from_eeprom(); - return false; - } - // exit immediate if no points - if (_boundary == nullptr) { - return false; - } - if (!_valid) { - // fence isn't valid - can't breach it?! + if (!loaded()) { return false; } - // start from 2nd point as boundary contains return point (as first point) - uint8_t start_num = 1; + // check we are inside each inclusion zone: + for (uint8_t i=0; i<_num_loaded_inclusion_boundaries; i++) { + const InclusionBoundary &boundary = _loaded_inclusion_boundary[i]; + if (Polygon_outside(pos_cm, boundary.points, boundary.count)) { + return true; + } + } - // check location is within the fence - return Polygon_outside(location, &_boundary[start_num], _boundary_num_points-start_num); + // check we are outside each exclusion zone: + for (uint8_t i=0; i<_num_loaded_exclusion_boundaries; i++) { + const ExclusionBoundary &boundary = _loaded_exclusion_boundary[i]; + if (!Polygon_outside(pos_cm, boundary.points, boundary.count)) { + return true; + } + } + + // check circular excludes + for (uint8_t i=0; i<_num_loaded_circle_exclusion_boundaries; i++) { + const ExclusionCircle &circle = _loaded_circle_exclusion_boundary[i]; + const Vector2f diff_cm = pos_cm - circle.pos_cm; + const float diff_cm_squared = diff_cm.length_squared(); + if (diff_cm_squared < sq(circle.radius*100.0f)) { + return true; + } + } + + // check circular includes + for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) { + const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i]; + const Vector2f diff_cm = pos_cm - circle.pos_cm; + const float diff_cm_squared = diff_cm.length_squared(); + if (diff_cm_squared > sq(circle.radius*100.0f)) { + return true; + } + } + + // no fence breached + return false; +} + +bool AC_PolyFence_loader::formatted() const +{ + return (fence_storage.read_uint8(0) == new_fence_storage_magic && + fence_storage.read_uint8(1) == 0 && + fence_storage.read_uint8(2) == 0 && + fence_storage.read_uint8(3) == 0); +} + +uint16_t AC_PolyFence_loader::max_items() const +{ + // this is 84 items on PixHawk + return MIN(255U, fence_storage.size() / sizeof(Vector2l)); +} + +bool AC_PolyFence_loader::format() +{ + uint16_t offset = 0; + fence_storage.write_uint32(offset, 0); + fence_storage.write_uint8(offset, new_fence_storage_magic); + offset += 4; + void_index(); + _eeprom_fence_count = 0; + _eeprom_item_count = 0; + return write_eos_to_storage(offset); +} + +bool AC_PolyFence_loader::convert_to_new_storage() +{ + // sanity check total + _total = constrain_int16(_total, 0, max_items()); + // FIXME: ensure the fence was closed and don't load it if it was not + if (_total < 5) { + // fence was invalid. Just format it and move on + return format(); + } + + if (hal.util->available_memory() < 100U + _total * sizeof(Vector2l)) { + return false; + } + + Vector2l *_tmp_boundary = new Vector2l[_total]; + if (_tmp_boundary == nullptr) { + return false; + } + + // load each point from eeprom + bool ret = false; + for (uint16_t index=0; index<_total; index++) { + // load boundary point as lat/lon point + if (!load_point_from_eeprom(index, _tmp_boundary[index])) { + goto out; + } + } + + // now store: + if (!format()) { + goto out; + } + { + uint16_t offset = 4; // skip magic + // write return point + if (!write_type_to_storage(offset, AC_PolyFenceType::RETURN_POINT)) { + return false; + } + if (!write_latlon_to_storage(offset, _tmp_boundary[0])) { + return false; + } + // write out polygon fence + fence_storage.write_uint8(offset, (uint8_t)AC_PolyFenceType::POLYGON_INCLUSION); + offset++; + fence_storage.write_uint8(offset, (uint8_t)_total-2); + offset++; + for (uint8_t i=1; i<_total-1; i++) { + if (!write_latlon_to_storage(offset, _tmp_boundary[i])) { + goto out; + } + } + // write eos marker + if (!write_eos_to_storage(offset)) { + goto out; + } + } + + ret = true; + +out: + delete[] _tmp_boundary; + return ret; +} + +bool AC_PolyFence_loader::read_scaled_latlon_from_storage(const Location &origin, uint16_t &read_offset, Vector2f &pos_cm) +{ + Location tmp_loc; + tmp_loc.lat = fence_storage.read_uint32(read_offset); + read_offset += 4; + tmp_loc.lng = fence_storage.read_uint32(read_offset); + read_offset += 4; + pos_cm = origin.get_distance_NE(tmp_loc) * 100.0f; + return true; +} + +bool AC_PolyFence_loader::read_polygon_from_storage(const Location &origin, uint16_t &read_offset, const uint8_t vertex_count, Vector2f *&next_storage_point) +{ + for (uint8_t i=0; i fence_storage.size()) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("did not find end-of-storage-marker before running out of space"); +#endif + return false; + } + const AC_PolyFenceType type = (AC_PolyFenceType)fence_storage.read_uint8(read_offset); + // validate what we've just pulled back from storage: + switch (type) { + case AC_PolyFenceType::END_OF_STORAGE: + case AC_PolyFenceType::POLYGON_INCLUSION: + case AC_PolyFenceType::POLYGON_EXCLUSION: + case AC_PolyFenceType::CIRCLE_INCLUSION: + case AC_PolyFenceType::CIRCLE_EXCLUSION: + case AC_PolyFenceType::RETURN_POINT: + break; + default: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Fence corrupt (offset=%u)", read_offset); +#endif + gcs().send_text(MAV_SEVERITY_WARNING, "Fence corrupt"); + return false; + } + + scan_fn(type, read_offset); + read_offset++; + switch (type) { + case AC_PolyFenceType::END_OF_STORAGE: + _eos_offset = read_offset-1; + all_done = true; + break; + case AC_PolyFenceType::POLYGON_INCLUSION: + case AC_PolyFenceType::POLYGON_EXCLUSION: { + const uint8_t vertex_count = fence_storage.read_uint8(read_offset); + read_offset += 1; // for the count we just read + read_offset += vertex_count*8; + break; + } + case AC_PolyFenceType::CIRCLE_INCLUSION: + case AC_PolyFenceType::CIRCLE_EXCLUSION: { + read_offset += 8; // for latlon + read_offset += 4; // for radius + break; + } + case AC_PolyFenceType::RETURN_POINT: + read_offset += 8; // for latlon + break; + } + } + return true; +} + +// note read_offset here isn't const and ALSO is not a reference +void AC_PolyFence_loader::scan_eeprom_count_fences(const AC_PolyFenceType type, uint16_t read_offset) +{ + if (type == AC_PolyFenceType::END_OF_STORAGE) { + return; + } + _eeprom_fence_count++; + switch (type) { + case AC_PolyFenceType::END_OF_STORAGE: + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + break; + case AC_PolyFenceType::POLYGON_EXCLUSION: + case AC_PolyFenceType::POLYGON_INCLUSION: { + const uint8_t vertex_count = fence_storage.read_uint8(read_offset+1); // skip type + _eeprom_item_count += vertex_count; + break; + } + case AC_PolyFenceType::CIRCLE_INCLUSION: + case AC_PolyFenceType::CIRCLE_EXCLUSION: + case AC_PolyFenceType::RETURN_POINT: + _eeprom_item_count++; + break; + } +} + +bool AC_PolyFence_loader::count_eeprom_fences() +{ + _eeprom_fence_count = 0; + _eeprom_item_count = 0; + const bool ret = scan_eeprom(FUNCTOR_BIND_MEMBER(&AC_PolyFence_loader::scan_eeprom_count_fences, void, const AC_PolyFenceType, uint16_t)); + return ret; +} + +void AC_PolyFence_loader::scan_eeprom_index_fences(const AC_PolyFenceType type, uint16_t read_offset) +{ + if (_index == nullptr) { + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return; + } + if (type == AC_PolyFenceType::END_OF_STORAGE) { + return; + } + FenceIndex &index = _index[_num_fences++]; + index.type = type; + index.storage_offset = read_offset; + switch (type) { + case AC_PolyFenceType::END_OF_STORAGE: + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + break; + case AC_PolyFenceType::POLYGON_EXCLUSION: + case AC_PolyFenceType::POLYGON_INCLUSION: { + const uint8_t vertex_count = fence_storage.read_uint8(read_offset+1); + index.count = vertex_count; + break; + } + case AC_PolyFenceType::CIRCLE_INCLUSION: + case AC_PolyFenceType::CIRCLE_EXCLUSION: + index.count = 1; + break; + case AC_PolyFenceType::RETURN_POINT: + index.count = 1; + break; + } +} + +bool AC_PolyFence_loader::index_eeprom() +{ + if (!formatted()) { + if (!convert_to_new_storage()) { + return false; + } + } + + if (!count_eeprom_fences()) { + return false; + } + if (_eeprom_fence_count == 0) { + _load_attempted = false; + return true; + } + + void_index(); + + Debug("Fence: Allocating %u bytes for index", + (unsigned)(_eeprom_fence_count*sizeof(FenceIndex))); + _index = new FenceIndex[_eeprom_fence_count]; + if (_index == nullptr) { + return false; + } + + _num_fences = 0; + if (!scan_eeprom(FUNCTOR_BIND_MEMBER(&AC_PolyFence_loader::scan_eeprom_index_fences, void, const AC_PolyFenceType, uint16_t))) { + void_index(); + return false; + } + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (_num_fences != _eeprom_fence_count) { + AP_HAL::panic("indexed fences not equal to eeprom fences"); + } +#endif + + _load_attempted = false; + + return true; +} + +bool AC_PolyFence_loader::check_indexed() +{ + if (!_index_attempted) { + _indexed = index_eeprom(); + _index_attempted = true; + } + return _indexed; +} + +void AC_PolyFence_loader::unload() +{ + delete[] _loaded_offsets_from_origin; + _loaded_offsets_from_origin = nullptr; + + delete[] _loaded_inclusion_boundary; + _loaded_inclusion_boundary = nullptr; + _num_loaded_inclusion_boundaries = 0; + + delete[] _loaded_exclusion_boundary; + _loaded_exclusion_boundary = nullptr; + _num_loaded_exclusion_boundaries = 0; + + delete[] _loaded_circle_inclusion_boundary; + _loaded_circle_inclusion_boundary = nullptr; + _num_loaded_circle_inclusion_boundaries = 0; + + delete[] _loaded_circle_exclusion_boundary; + _loaded_circle_exclusion_boundary = nullptr; + _num_loaded_circle_exclusion_boundaries = 0; + + _loaded_return_point = nullptr; + _load_time_ms = 0; +} + +// return the number of fences of type type in the index: +uint16_t AC_PolyFence_loader::index_fence_count(const AC_PolyFenceType type) +{ + uint16_t ret = 0; + for (uint8_t i=0; i<_eeprom_fence_count; i++) { + const FenceIndex &index = _index[i]; + if (index.type == type) { + ret++; + } + } + return ret; +} + +uint16_t AC_PolyFence_loader::sum_of_polygon_point_counts_and_returnpoint() +{ + uint16_t ret = 0; + for (uint8_t i=0; i<_eeprom_fence_count; i++) { + const FenceIndex &index = _index[i]; + switch (index.type) { + case AC_PolyFenceType::CIRCLE_INCLUSION: + case AC_PolyFenceType::CIRCLE_EXCLUSION: + break; + case AC_PolyFenceType::RETURN_POINT: + ret += 1; + break; + case AC_PolyFenceType::POLYGON_INCLUSION: + case AC_PolyFenceType::POLYGON_EXCLUSION: + ret += index.count; + break; + case AC_PolyFenceType::END_OF_STORAGE: + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + break; + } + } + return ret; +} + +bool AC_PolyFence_loader::load_from_eeprom() +{ + if (!check_indexed()) { + return false; + } + + if (_load_attempted) { + return _load_time_ms != 0; + } + + struct Location ekf_origin{}; + if (!AP::ahrs().get_origin(ekf_origin)) { +// Debug("fence load requires origin"); + return false; + } + + _load_attempted = true; + + // find indexes of each fence: + if (!get_loaded_fence_semaphore().take(1)) { + return false; + } + + unload(); + + if (_eeprom_item_count == 0) { + get_loaded_fence_semaphore().give(); + _load_time_ms = AP_HAL::millis(); + return true; + } + + { // allocate array to hold offsets-from-origin + const uint16_t count = sum_of_polygon_point_counts_and_returnpoint(); + Debug("Fence: Allocating %u bytes for points", + (unsigned)(count * sizeof(Vector2f))); + _loaded_offsets_from_origin = new Vector2f[count]; + if (_loaded_offsets_from_origin == nullptr) { + unload(); + get_loaded_fence_semaphore().give(); + return false; + } + } + + // FIXME: find some way of factoring out all of these allocation routines. + + { // allocate storage for inclusion polyfences: + const uint8_t count = index_fence_count(AC_PolyFenceType::POLYGON_INCLUSION); + Debug("Fence: Allocating %u bytes for inc. fences", + (unsigned)(count * sizeof(InclusionBoundary))); + _loaded_inclusion_boundary = new InclusionBoundary[count]; + if (_loaded_inclusion_boundary == nullptr) { + unload(); + get_loaded_fence_semaphore().give(); + return false; + } + } + + { // allocate storage for exclusion polyfences: + const uint8_t count = index_fence_count(AC_PolyFenceType::POLYGON_EXCLUSION); + Debug("Fence: Allocating %u bytes for exc. fences", + (unsigned)(count * sizeof(ExclusionBoundary))); + _loaded_exclusion_boundary = new ExclusionBoundary[count]; + if (_loaded_exclusion_boundary == nullptr) { + unload(); + get_loaded_fence_semaphore().give(); + return false; + } + } + + { // allocate storage for circular inclusion fences: + uint8_t count = index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION); + Debug("Fence: Allocating %u bytes for circ. inc. fences", + (unsigned)(count * sizeof(InclusionCircle))); + _loaded_circle_inclusion_boundary = new InclusionCircle[count]; + if (_loaded_circle_inclusion_boundary == nullptr) { + unload(); + get_loaded_fence_semaphore().give(); + return false; + } + } + + { // allocate storage for circular exclusion fences: + uint8_t count = index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION); + Debug("Fence: Allocating %u bytes for circ. exc. fences", + (unsigned)(count * sizeof(ExclusionCircle))); + _loaded_circle_exclusion_boundary = new ExclusionCircle[count]; + if (_loaded_circle_exclusion_boundary == nullptr) { + unload(); + get_loaded_fence_semaphore().give(); + return false; + } + } + + Vector2f *next_storage_point = _loaded_offsets_from_origin; + + // use index to load fences from eeprom + bool storage_valid = true; + for (uint8_t i=0; i<_eeprom_fence_count; i++) { + if (!storage_valid) { + break; + } + const FenceIndex &index = _index[i]; + uint16_t storage_offset = index.storage_offset; + storage_offset += 1; // skip type + switch (index.type) { + case AC_PolyFenceType::END_OF_STORAGE: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("indexed end of storage found"); +#endif + storage_valid = false; + break; + case AC_PolyFenceType::POLYGON_INCLUSION: { + // FIXME: consider factoring this with the EXCLUSION case + InclusionBoundary &boundary = _loaded_inclusion_boundary[_num_loaded_inclusion_boundaries]; + boundary.points = next_storage_point; + boundary.count = index.count; + if (index.count < 3) { + gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count"); + storage_valid = false; + break; + } + storage_offset += 1; // skip vertex count + if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed"); + storage_valid = false; + break; + } + _num_loaded_inclusion_boundaries++; + break; + } + case AC_PolyFenceType::POLYGON_EXCLUSION: { + ExclusionBoundary &boundary = _loaded_exclusion_boundary[_num_loaded_exclusion_boundaries]; + boundary.points = next_storage_point; + boundary.count = index.count; + if (index.count < 3) { + gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count"); + storage_valid = false; + break; + } + storage_offset += 1; // skip vertex count + if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed"); + storage_valid = false; + break; + } + _num_loaded_exclusion_boundaries++; + break; + } + case AC_PolyFenceType::CIRCLE_EXCLUSION: { + ExclusionCircle &circle = _loaded_circle_exclusion_boundary[_num_loaded_circle_exclusion_boundaries]; + if (!read_scaled_latlon_from_storage(ekf_origin, storage_offset, circle.pos_cm)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed"); + storage_valid = false; + break; + } + // now read the radius + circle.radius = fence_storage.read_uint32(storage_offset); + if (circle.radius <= 0) { + gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius"); + storage_valid = false; + break; + } + _num_loaded_circle_exclusion_boundaries++; + break; + } + case AC_PolyFenceType::CIRCLE_INCLUSION: { + InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries]; + if (!read_scaled_latlon_from_storage(ekf_origin, storage_offset, circle.pos_cm)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed"); + storage_valid = false; + break; + } + // now read the radius + circle.radius = fence_storage.read_uint32(storage_offset); + if (circle.radius <= 0) { + gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius"); + storage_valid = false; + break; + } + _num_loaded_circle_inclusion_boundaries++; + break; + } + case AC_PolyFenceType::RETURN_POINT: + if (_loaded_return_point != nullptr) { + gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found"); + storage_valid = false; + break; + } + _loaded_return_point = next_storage_point; + if (!read_scaled_latlon_from_storage(ekf_origin, storage_offset, *next_storage_point)) { + storage_valid = false; + gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed"); + break; + } + next_storage_point++; + break; + } + } + + if (!storage_valid) { + unload(); + get_loaded_fence_semaphore().give(); + return false; + } + + _load_time_ms = AP_HAL::millis(); + + get_loaded_fence_semaphore().give(); + return true; +} + +/// 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* AC_PolyFence_loader::get_exclusion_polygon(uint16_t index, uint16_t &num_points) const +{ + if (index >= _num_loaded_exclusion_boundaries) { + num_points = 0; + return nullptr; + } + const ExclusionBoundary &boundary = _loaded_exclusion_boundary[index]; + num_points = boundary.count; + + return boundary.points; +} + +/// 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* AC_PolyFence_loader::get_inclusion_polygon(uint16_t index, uint16_t &num_points) const +{ + if (index >= _num_loaded_inclusion_boundaries) { + num_points = 0; + return nullptr; + } + const InclusionBoundary &boundary = _loaded_inclusion_boundary[index]; + num_points = boundary.count; + + return boundary.points; +} + +/// returns the specified exclusion circle +/// circle center offsets in cm from EKF origin in NE frame, radius is in meters +bool AC_PolyFence_loader::get_exclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const +{ + if (index >= _num_loaded_circle_exclusion_boundaries) { + return false; + } + center_pos_cm = _loaded_circle_exclusion_boundary[index].pos_cm; + radius = _loaded_circle_exclusion_boundary[index].radius; + return true; +} + +/// returns the specified inclusion circle +/// circle centre offsets in cm from EKF origin in NE frame, radius is in meters +bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const +{ + if (index >= _num_loaded_circle_inclusion_boundaries) { + return false; + } + center_pos_cm = _loaded_circle_inclusion_boundary[index].pos_cm; + radius = _loaded_circle_inclusion_boundary[index].radius; + return true; +} + +bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint16_t count) const +{ + // validate the fence items... + AC_PolyFenceType expecting_type = AC_PolyFenceType::END_OF_STORAGE; + uint16_t expected_type_count = 0; + uint16_t orig_expected_type_count = 0; + bool seen_return_point = false; + + for (uint16_t i=0; i fence_storage.size()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Fence exceeds storage size"); + return false; + } + + if (!format()) { + return false; + } + + uint8_t total_vertex_count = 0; + uint16_t offset = 4; // skipping magic + uint8_t vertex_count = 0; + for (uint16_t i=0; istorage_offset + 1; + return read_latlon_from_storage(read_offset, ret); + } + + const FenceIndex *inc = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION); + if (inc == nullptr) { + return false; + } + + // we found an inclusion fence but not a return point. Calculate + // and return the centroid. Note that this may not actually be + // inside all inclusion fences... + uint16_t offset = inc->storage_offset; + if ((AC_PolyFenceType)fence_storage.read_uint8(offset) != AC_PolyFenceType::POLYGON_INCLUSION) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("wrong type at offset"); +#endif + return false; + } + offset++; + const uint8_t count = fence_storage.read_uint8(offset); + if (count < 3) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("invalid count found"); +#endif + return false; + } + offset++; + Vector2l min_loc; + if (!read_latlon_from_storage(offset, min_loc)) { + return false; + } + if (min_loc.is_zero()) { + return false; + } + Vector2l max_loc = min_loc; + for (uint8_t i=1; i max_loc.x) { + max_loc.x = new_loc.x; + } + if (new_loc.y > max_loc.y) { + max_loc.y = new_loc.y; + } + } + + ret.x = ((min_loc.x+max_loc.x)/2); + ret.y = ((min_loc.y+max_loc.y)/2); + + return true; +} +#endif + +AC_PolyFence_loader::FenceIndex *AC_PolyFence_loader::find_first_fence(const AC_PolyFenceType type) const +{ + if (_index == nullptr) { + return nullptr; + } + for (uint8_t i=0; i<_num_fences; i++) { + if (_index[i].type == type) { + return &_index[i]; + } + } + return nullptr; +} + +#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT +void AC_PolyFence_loader::handle_msg_fetch_fence_point(GCS_MAVLINK &link, const mavlink_message_t& msg) +{ + if (!check_indexed()) { + return; + } + if (!contains_compatible_fence()) { + link.send_text(MAV_SEVERITY_WARNING, "Vehicle contains advanced fences"); + return; + } + + if (_total != 0 && _total < 5) { + link.send_text(MAV_SEVERITY_WARNING, "Invalid FENCE_TOTAL"); + return; + } + + mavlink_fence_fetch_point_t packet; + mavlink_msg_fence_fetch_point_decode(&msg, &packet); + + if (packet.idx >= _total) { + link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, index past total(%u >= %u)", packet.idx, _total.get()); + return; + } + + mavlink_fence_point_t ret_packet{}; + ret_packet.target_system = msg.sysid; + ret_packet.target_component = msg.compid; + ret_packet.idx = packet.idx; + ret_packet.count = _total; + + if (packet.idx == 0) { + // return point + Vector2l ret; + if (get_return_point(ret)) { + ret_packet.lat = ret.x * 1.0e-7f; + ret_packet.lng = ret.y * 1.0e-7f; + } else { + link.send_text(MAV_SEVERITY_WARNING, "Failed to get return point"); + } + } else { + // find the inclusion fence: + const FenceIndex *inclusion_fence = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION); + if (inclusion_fence == nullptr) { + // nothing stored yet; just send back zeroes + ret_packet.lat = 0; + ret_packet.lng = 0; + } else { + uint8_t fencepoint_offset; // 1st idx is return point + if (packet.idx == _total-1) { + // the is the loop closure point - send the first point again + fencepoint_offset = 0; + } else { + fencepoint_offset = packet.idx - 1; + } + if (fencepoint_offset >= inclusion_fence->count) { + // we haven't been given a value for this item yet; we will return zeroes + } else { + uint16_t storage_offset = inclusion_fence->storage_offset; + storage_offset++; // skip over type + storage_offset++; // skip over count + storage_offset += 8*fencepoint_offset; // move to point we're interested in + Vector2l bob; + if (!read_latlon_from_storage(storage_offset, bob)) { + link.send_text(MAV_SEVERITY_WARNING, "Fence read failed"); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("read failure"); +#endif + return; + } + ret_packet.lat = bob[0] * 1.0e-7f; + ret_packet.lng = bob[1] * 1.0e-7f; + } + } + } + + link.send_message(MAVLINK_MSG_ID_FENCE_POINT, (const char*)&ret_packet); +} + +AC_PolyFence_loader::FenceIndex *AC_PolyFence_loader::get_or_create_return_point() +{ + if (!check_indexed()) { + return nullptr; + } + FenceIndex *return_point = find_first_fence(AC_PolyFenceType::RETURN_POINT); + if (return_point != nullptr) { + return return_point; + } + + // if the inclusion fence exists we will move it in storage to + // avoid having to continually shift the return point forward as + // we receive fence points + uint16_t offset; + const FenceIndex *inclusion_fence = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION); + if (inclusion_fence != nullptr) { + offset = inclusion_fence->storage_offset; + // the "9"s below represent the size of a return point in storage + for (uint8_t i=0; icount; i++) { + // we are shifting the last fence point first - so 'i=0' + // means the last point stored. + const uint16_t point_storage_offset = offset + 2 + (inclusion_fence->count-1-i) * 8; + Vector2l latlon; + uint16_t tmp_read_offs = point_storage_offset; + if (!read_latlon_from_storage(tmp_read_offs, latlon)) { + return nullptr; + } + uint16_t write_offset = point_storage_offset + 9; + if (!write_latlon_to_storage(write_offset, latlon)) { + return nullptr; + } + } + // read/write the count: + const uint8_t count = fence_storage.read_uint8(inclusion_fence->storage_offset+1); + fence_storage.write_uint8(inclusion_fence->storage_offset + 1 + 9, count); + // read/write the type: + const uint8_t t = fence_storage.read_uint8(inclusion_fence->storage_offset); + fence_storage.write_uint8(inclusion_fence->storage_offset + 9, t); + + uint16_t write_offset = offset + 2 + 8*inclusion_fence->count + 9; + if (!write_eos_to_storage(write_offset)) { + return nullptr; + } + } else { + if (fence_storage.read_uint8(_eos_offset) != (uint8_t)AC_PolyFenceType::END_OF_STORAGE) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Expected end-of-storage marker at offset=%u", + _eos_offset); +#endif + return nullptr; + } + offset = _eos_offset; + } + + if (!write_type_to_storage(offset, AC_PolyFenceType::RETURN_POINT)) { + return nullptr; + } + if (!write_latlon_to_storage(offset, Vector2l{0, 0})) { + return nullptr; + } + if (inclusion_fence == nullptr) { + if (!write_eos_to_storage(offset)) { + return nullptr; + } + } + + if (!index_eeprom()) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Failed to index eeprom after moving inclusion fence for return point"); +#endif + return nullptr; + } + + return_point = find_first_fence(AC_PolyFenceType::RETURN_POINT); + if (return_point == nullptr) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Failed to get return point after indexing"); +#endif + } + return return_point; +} + +AC_PolyFence_loader::FenceIndex *AC_PolyFence_loader::get_or_create_include_fence() +{ + if (!check_indexed()) { + return nullptr; + } + FenceIndex *inclusion = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION); + if (inclusion != nullptr) { + return inclusion; + } + if (_total < 5) { + return nullptr; + } + if (!write_type_to_storage(_eos_offset, AC_PolyFenceType::POLYGON_INCLUSION)) { + return nullptr; + } + fence_storage.write_uint8(_eos_offset, 0); + _eos_offset++; + if (!write_eos_to_storage(_eos_offset)) { + return nullptr; + } + + if (!index_eeprom()) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Failed to index eeprom after creating fence"); +#endif + return nullptr; + } + AC_PolyFence_loader::FenceIndex *ret = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (ret == nullptr) { + AP_HAL::panic("Failed to index eeprom after creating fence"); + } +#endif + return ret; +} + +void AC_PolyFence_loader::handle_msg_fence_point(GCS_MAVLINK &link, const mavlink_message_t& msg) +{ + if (!check_indexed()) { + return; + } + + mavlink_fence_point_t packet; + mavlink_msg_fence_point_decode(&msg, &packet); + + if (_total != 0 && _total < 5) { + link.send_text(MAV_SEVERITY_WARNING, "Invalid FENCE_TOTAL"); + return; + } + + if (packet.count != _total) { + link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, bad count (%u vs %u)", packet.count, _total.get()); + return; + } + + if (packet.idx >= _total) { + // this is a protocol failure + link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, index past total (%u >= %u)", packet.idx, _total.get()); + return; + } + + if (!check_latlng(packet.lat, packet.lng)) { + link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, bad lat or lng"); + return; + } + + if (!contains_compatible_fence()) { + // the GCS has started to upload using the old protocol; + // ensure we can accept it. We must be able to index the + // fence, so it must be valid (minimum number of points) + if (!format()) { + return; + } + } + + const Vector2l point{ + (int32_t)(packet.lat*1.0e7f), + (int32_t)(packet.lng*1.0e7f) + }; + + if (packet.idx == 0) { + // this is the return point; if we have a return point then + // update it, otherwise create a return point fence thingy + const FenceIndex *return_point = get_or_create_return_point(); + if (return_point == nullptr) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Didn't get return point"); +#endif + return; + } + uint16_t offset = return_point->storage_offset; + offset++; // don't overwrite the type! + if (!write_latlon_to_storage(offset, point)) { + link.send_text(MAV_SEVERITY_WARNING, "PolyFence: storage write failed"); + return; + } + } else if (packet.idx == _total-1) { + // this is the fence closing point; don't store it, and don't + // check it against the first point in the fence as we may be + // receiving the fence points out of order. Note that if the + // GCS attempts to read this back before sending the first + // point they will get 0s. + } else { + const FenceIndex *inclusion_fence = get_or_create_include_fence(); + if (inclusion_fence == nullptr) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("no inclusion fences found"); +#endif + return; + } + uint16_t offset = inclusion_fence->storage_offset; + offset++; // skip type + if (packet.idx > inclusion_fence->count) { + // expand the storage space + fence_storage.write_uint8(offset, packet.idx); // remembering that idx[0] is the return point.... + } + offset++; // move past number of points + offset += (packet.idx-1)*8; + if (!write_latlon_to_storage(offset, point)) { + link.send_text(MAV_SEVERITY_WARNING, "PolyFence: storage write failed"); + return; + } + if (_eos_offset < offset) { + if (!write_eos_to_storage(offset)) { + return; + } + } + void_index(); + } +} + +bool AC_PolyFence_loader::contains_compatible_fence() const +{ + // must contain a single inclusion fence with an optional return point + if (_index == nullptr) { + // this indicates no boundary points present + return true; + } + bool seen_return_point = false; + bool seen_poly_inclusion = false; + for (uint16_t i=0; i<_num_fences; i++) { + switch (_index[i].type) { + case AC_PolyFenceType::END_OF_STORAGE: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("end-of-storage marker found in loaded list"); +#endif + return false; + case AC_PolyFenceType::POLYGON_INCLUSION: + if (seen_poly_inclusion) { + return false; + } + seen_poly_inclusion = true; + break; + case AC_PolyFenceType::POLYGON_EXCLUSION: + case AC_PolyFenceType::CIRCLE_INCLUSION: + case AC_PolyFenceType::CIRCLE_EXCLUSION: + return false; + case AC_PolyFenceType::RETURN_POINT: + if (seen_return_point) { + return false; + } + seen_return_point = true; + break; + } + } + return true; +} + +#endif // AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT + +bool AC_PolyFence_loader::write_eos_to_storage(uint16_t &offset) +{ + if (!write_type_to_storage(offset, AC_PolyFenceType::END_OF_STORAGE)) { + return false; + } + _eos_offset = offset - 1; // should point to the marker + return true; +} + +/// handler for polygon fence messages with GCS +void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg) +{ + switch (msg.msgid) { +#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT + case MAVLINK_MSG_ID_FENCE_POINT: + handle_msg_fence_point(link, msg); + break; + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: + handle_msg_fetch_fence_point(link, msg); + break; +#endif + default: + break; + } +} + +void AC_PolyFence_loader::update() +{ +#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT + // if an older GCS sets the fence point count to zero then clear the fence + if (_old_total != _total) { + _old_total = _total; + if (_total == 0 && _eeprom_fence_count) { + if (!format()) { + // we are in all sorts of trouble + return; + } + } + } +#endif + if (!load_from_eeprom()) { + return; + } } diff --git a/libraries/AC_Fence/AC_PolyFence_loader.h b/libraries/AC_Fence/AC_PolyFence_loader.h index 925afae337..9521e21c6a 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.h +++ b/libraries/AC_Fence/AC_PolyFence_loader.h @@ -1,8 +1,33 @@ #pragma once #include +#include #include -#include +#include + +#define AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT 1 + +enum class AC_PolyFenceType { + END_OF_STORAGE = 99, + POLYGON_INCLUSION = 98, + POLYGON_EXCLUSION = 97, + CIRCLE_EXCLUSION = 96, + RETURN_POINT = 95, + CIRCLE_INCLUSION = 94, +}; + +// 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 { @@ -15,58 +40,366 @@ public: AC_PolyFence_loader(const AC_PolyFence_loader &other) = delete; AC_PolyFence_loader &operator=(const AC_PolyFence_loader&) = delete; - /// returns pointer to array of polygon points and num_points is - /// filled in with the total number. This does not include the - /// return point or the closing point. - Vector2f* get_boundary_points(uint16_t& num_points) const; + 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; + + /// + /// mavlink + /// /// handler for polygon fence messages with GCS - void handle_msg(GCS_MAVLINK &link, mavlink_message_t* msg); + void handle_msg(class GCS_MAVLINK &link, const mavlink_message_t& msg); - bool breached(); - // returns true if location is outside the boundary - bool breached(const Location& loc); + // 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 boundary is valid - bool valid() const { - return _valid; + // returns true if a polygonal include fence could be returned + bool inclusion_boundary_available() const WARN_IF_UNUSED { + return _num_loaded_inclusion_boundaries != 0; } - // returns the system in in millis when the boundary was last updated - uint32_t update_ms() const { - return _update_ms; + // 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(); + private: - bool breached(const Vector2f& location); - /// load polygon points stored in eeprom into boundary array and - /// perform validation. returns true if load successfully - /// completed - bool load_from_eeprom(); + // multi-thread access support + HAL_Semaphore _loaded_fence_sem; - // maximum number of fence points we can store in eeprom - uint8_t max_points() const; + // breached(Vector2f&) - returns true of pos_cm (an offset in cm from the EKF origin) breaches any fence + bool breached(const Vector2f& pos_cm) const WARN_IF_UNUSED; - // create buffer to hold copy of eeprom points in RAM - // returns nullptr if not enough memory can be allocated - void* create_point_array(uint8_t element_size); + /* + * 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; + + class InclusionBoundary { + public: + Vector2f *points; // pointer into the _loaded_offsets_from_origin 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 + 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; + + class ExclusionCircle { + public: + Vector2f pos_cm; + float radius; + }; + ExclusionCircle *_loaded_circle_exclusion_boundary; + uint8_t _num_loaded_circle_exclusion_boundaries; + + class InclusionCircle { + public: + Vector2f pos_cm; + 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; + + // read_scaled_latlon_from_storage - reads a latitude/longitude + // from offset in permanent storage, transforms them into an + // offset-from-origin and deposits the result into pos_cm. + // read_offset is increased by the storage space used by the + // latitude/longitude + bool read_scaled_latlon_from_storage(const Location &origin, + uint16_t &read_offset, + 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) 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); + bool load_point_from_eeprom(uint16_t i, Vector2l& point) WARN_IF_UNUSED; - // save a fence point to eeprom, returns true on successful save - bool save_point_to_eeprom(uint16_t i, const Vector2l& point); - // update the validity flag: - bool calculate_boundary_valid() const; +#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; - Vector2f *_boundary; // array of boundary points. Note: point 0 is the return point - uint8_t _boundary_num_points; // number of points in the boundary array (should equal _total parameter after load has completed) - bool _create_attempted; // true if we have attempted to create the boundary array - bool _valid; // true if boundary forms a closed polygon - uint32_t _update_ms; // system time of last update to the boundary + // 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); + +#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 + + // _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); +};