Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AC_Fence/AC_PolyFence_loader.cpp

1657 lines
55 KiB
C++
Raw Blame History

#include "AC_PolyFence_loader.h"
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
#define POLYFENCE_LOADER_DEBUGGING 0
#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);
void AC_PolyFence_loader::init()
{
if (!check_indexed()) {
// tell the user, perhaps?
}
_old_total = _total;
}
bool AC_PolyFence_loader::find_index_for_seq(const uint16_t seq, const FenceIndex *&entry, uint16_t &i) const
{
if (_index == nullptr) {
return false;
}
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) {
INTERNAL_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_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
case AC_PolyFenceType::CIRCLE_INCLUSION:
case AC_PolyFenceType::CIRCLE_EXCLUSION:
if (delta != 0) {
INTERNAL_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) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
}
break;
case AC_PolyFenceType::END_OF_STORAGE:
INTERNAL_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_float(offset);
break;
case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
if (!read_latlon_from_storage(offset, item.loc)) {
return false;
}
item.radius = fence_storage.read_uint32(offset);
// magically change int item into a float item:
if (type == AC_PolyFenceType::CIRCLE_INCLUSION_INT) {
item.type = AC_PolyFenceType::CIRCLE_INCLUSION;
} else {
item.type = AC_PolyFenceType::CIRCLE_EXCLUSION;
}
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
INTERNAL_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) const
{
// sanity check index
if (i >= max_items()) {
return false;
}
// read fence point
point.x = fence_storage.read_uint32(i * sizeof(Vector2l));
point.y = fence_storage.read_uint32(i * sizeof(Vector2l) + sizeof(uint32_t));
return true;
}
bool AC_PolyFence_loader::breached() const
{
struct Location loc;
if (!AP::ahrs().get_position(loc)) {
return false;
}
return breached(loc);
}
// check if a position (expressed as lat/lng) is within the boundary
// returns true if location is outside the boundary
bool AC_PolyFence_loader::breached(const Location& loc) const
{
if (!loaded()) {
return false;
}
Vector2l pos;
pos.x = loc.lat;
pos.y = loc.lng;
// 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, boundary.points_lla, boundary.count)) {
return true;
}
}
// 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, boundary.points_lla, boundary.count)) {
return true;
}
}
for (uint8_t i=0; i<_num_loaded_circle_exclusion_boundaries; i++) {
const ExclusionCircle &circle = _loaded_circle_exclusion_boundary[i];
Location circle_center;
circle_center.lat = circle.point.x;
circle_center.lng = circle.point.y;
const float diff_cm = loc.get_distance(circle_center)*100.0f;
if (diff_cm < circle.radius * 100.0f) {
return true;
}
}
for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) {
const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i];
Location circle_center;
circle_center.lat = circle.point.x;
circle_center.lng = circle.point.y;
const float diff_cm = loc.get_distance(circle_center)*100.0f;
if (diff_cm > 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::scale_latlon_from_origin(const Location &origin, const Vector2l &point, Vector2f &pos_cm)
{
Location tmp_loc;
tmp_loc.lat = point.x;
tmp_loc.lng = point.y;
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, Vector2l *&next_storage_point_lla)
{
for (uint8_t i=0; i<vertex_count; i++) {
// read from storage to lat/lon
if (!read_latlon_from_storage(read_offset, *next_storage_point_lla)) {
return false;
}
// convert lat/lon to position in cm from origin
if (!scale_latlon_from_origin(origin, *next_storage_point_lla, *next_storage_point)) {
return false;
}
next_storage_point_lla++;
next_storage_point++;
}
return true;
}
bool AC_PolyFence_loader::scan_eeprom(scan_fn_t scan_fn)
{
uint16_t read_offset = 0; // skipping reserved first 4 bytes
if (!formatted()) {
return false;
}
read_offset += 4;
bool all_done = false;
while (!all_done) {
if (read_offset > 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::CIRCLE_INCLUSION_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
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_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
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:
INTERNAL_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::CIRCLE_INCLUSION_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
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) {
INTERNAL_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:
INTERNAL_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_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
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_points_lla;
_loaded_points_lla = 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;
_loaded_return_point_lla = 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_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
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:
INTERNAL_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;
}
// find indexes of each fence:
if (!get_loaded_fence_semaphore().take_nonblocking()) {
return false;
}
_load_attempted = true;
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];
_loaded_points_lla = new Vector2l[count];
if (_loaded_offsets_from_origin == nullptr || _loaded_points_lla == 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);
count += index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION_INT)
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);
count += index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION_INT)
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;
Vector2l *next_storage_point_lla = _loaded_points_lla;
// 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.points_lla = next_storage_point_lla;
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, next_storage_point_lla)) {
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.points_lla = next_storage_point_lla;
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, next_storage_point_lla)) {
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_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION: {
ExclusionCircle &circle = _loaded_circle_exclusion_boundary[_num_loaded_circle_exclusion_boundaries];
if (!read_latlon_from_storage(storage_offset, circle.point)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
if (!scale_latlon_from_origin(ekf_origin, circle.point, circle.pos_cm)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
// now read the radius
if (index.type == AC_PolyFenceType::CIRCLE_EXCLUSION_INT) {
circle.radius = fence_storage.read_uint32(storage_offset);
} else {
circle.radius = fence_storage.read_float(storage_offset);
}
if (!is_positive(circle.radius)) {
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_INT:
case AC_PolyFenceType::CIRCLE_INCLUSION: {
InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries];
if (!read_latlon_from_storage(storage_offset, circle.point)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
if (!scale_latlon_from_origin(ekf_origin, circle.point, circle.pos_cm)){
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
// now read the radius
if (index.type == AC_PolyFenceType::CIRCLE_INCLUSION_INT) {
circle.radius = fence_storage.read_uint32(storage_offset);
} else {
circle.radius = fence_storage.read_float(storage_offset);
}
if (!is_positive(circle.radius)) {
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 (_loaded_return_point_lla != nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
storage_valid = false;
break;
}
_loaded_return_point_lla = next_storage_point_lla;
// Read the point from storage
if (!read_latlon_from_storage(storage_offset, *next_storage_point_lla)) {
storage_valid = false;
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
break;
}
if (!scale_latlon_from_origin(ekf_origin, *next_storage_point_lla, *next_storage_point)) {
storage_valid = false;
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
break;
}
next_storage_point++;
next_storage_point_lla++;
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 &center_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 &center_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::check_inclusion_circle_margin(float margin) const
{
// check circular includes
for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) {
const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i];
if (circle.radius < margin) {
// circle radius should never be less than margin
return false;
}
}
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<count; i++) {
bool validate_latlon = false;
switch (new_items[i].type) {
case AC_PolyFenceType::END_OF_STORAGE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("passed in an END_OF_STORAGE");
#endif
return false;
case AC_PolyFenceType::POLYGON_INCLUSION:
case AC_PolyFenceType::POLYGON_EXCLUSION:
if (new_items[i].vertex_count < 3) {
gcs().send_text(MAV_SEVERITY_WARNING, "Invalid vertex count (%u)", new_items[i].vertex_count);
return false;
}
if (expected_type_count == 0) {
expected_type_count = new_items[i].vertex_count;
orig_expected_type_count = expected_type_count;
expecting_type = new_items[i].type;
} else {
if (new_items[i].type != expecting_type) {
gcs().send_text(MAV_SEVERITY_WARNING, "Received incorrect vertex type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
return false;
} else if (new_items[i].vertex_count != orig_expected_type_count) {
gcs().send_text(MAV_SEVERITY_WARNING, "Unexpected vertex count want=%u got=%u\n", orig_expected_type_count, new_items[i].vertex_count);
return false;
}
}
expected_type_count--;
validate_latlon = true;
break;
case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
// should never have AC_PolyFenceItems of these types
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
case AC_PolyFenceType::CIRCLE_INCLUSION:
case AC_PolyFenceType::CIRCLE_EXCLUSION:
if (expected_type_count) {
gcs().send_text(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
return false;
}
if (!is_positive(new_items[i].radius)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Non-positive circle radius");
return false;
}
validate_latlon = true;
break;
case AC_PolyFenceType::RETURN_POINT:
if (expected_type_count) {
gcs().send_text(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
return false;
}
// spec says only one return point allowed
if (seen_return_point) {
gcs().send_text(MAV_SEVERITY_WARNING, "Multiple return points");
return false;
}
seen_return_point = true;
validate_latlon = true;
// TODO: ensure return point is within all fences and
// outside all exclusion zones
break;
}
if (validate_latlon) {
if (!check_latlng(new_items[i].loc[0], new_items[i].loc[1])) {
gcs().send_text(MAV_SEVERITY_WARNING, "Bad lat or lon");
return false;
}
}
}
if (expected_type_count) {
gcs().send_text(MAV_SEVERITY_INFO, "Incorrect item count");
return false;
}
return true;
}
uint16_t AC_PolyFence_loader::fence_storage_space_required(const AC_PolyFenceItem *new_items, uint16_t count)
{
uint16_t ret = 4; // for the format header
uint16_t i = 0;
while (i < count) {
ret += 1; // one byte for type
switch (new_items[i].type) {
case AC_PolyFenceType::POLYGON_INCLUSION:
case AC_PolyFenceType::POLYGON_EXCLUSION:
ret += 1 + 8 * new_items[i].vertex_count; // 1 count, 4 lat, 4 lon for each point
i += new_items[i].vertex_count - 1; // i is incremented down below
break;
case AC_PolyFenceType::END_OF_STORAGE:
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
// should never have AC_PolyFenceItems of these types
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
case AC_PolyFenceType::CIRCLE_INCLUSION:
case AC_PolyFenceType::CIRCLE_EXCLUSION:
ret += 12; // 4 radius, 4 lat, 4 lon
break;
case AC_PolyFenceType::RETURN_POINT:
ret += 8; // 4 lat, 4 lon
break;
}
i++;
}
return ret;
}
bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_t count)
{
if (!validate_fence(new_items, count)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Fence validation failed");
return false;
}
if (fence_storage_space_required(new_items, count) > 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; i<count; i++) {
const AC_PolyFenceItem new_item = new_items[i];
switch (new_item.type) {
case AC_PolyFenceType::POLYGON_INCLUSION:
case AC_PolyFenceType::POLYGON_EXCLUSION:
if (vertex_count == 0) {
// write out new polygon count
vertex_count = new_item.vertex_count;
total_vertex_count += vertex_count;
if (!write_type_to_storage(offset, new_item.type)) {
return false;
}
fence_storage.write_uint8(offset, vertex_count);
offset++;
}
vertex_count--;
if (!write_latlon_to_storage(offset, new_item.loc)) {
return false;
}
break;
case AC_PolyFenceType::END_OF_STORAGE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("asked to store end-of-storage marker");
#endif
return false;
case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
// should never have AC_PolyFenceItems of these types
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
case AC_PolyFenceType::CIRCLE_INCLUSION:
case AC_PolyFenceType::CIRCLE_EXCLUSION: {
total_vertex_count++; // useful to make number of lines in QGC file match FENCE_TOTAL
const bool store_as_int = (new_item.radius - int(new_item.radius) < 0.001);
AC_PolyFenceType store_type = new_item.type;
if (store_as_int) {
if (new_item.type == AC_PolyFenceType::CIRCLE_INCLUSION) {
store_type = AC_PolyFenceType::CIRCLE_INCLUSION_INT;
} else {
store_type = AC_PolyFenceType::CIRCLE_EXCLUSION_INT;
}
}
if (!write_type_to_storage(offset, store_type)) {
return false;
}
if (!write_latlon_to_storage(offset, new_item.loc)) {
return false;
}
// store the radius. If the radius is very close to an
// integer then we store it as an integer so users moving
// from 4.1 back to 4.0 might be less-disrupted.
if (store_as_int) {
fence_storage.write_uint32(offset, new_item.radius);
} else {
fence_storage.write_float(offset, new_item.radius);
}
offset += 4;
break;
}
case AC_PolyFenceType::RETURN_POINT:
if (!write_type_to_storage(offset, new_item.type)) {
return false;
}
if (!write_latlon_to_storage(offset, new_item.loc)) {
return false;
}
break;
}
}
if (!write_eos_to_storage(offset)) {
return false;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// sanity-check the EEPROM in SITL to make sure we can read what
// we've just written.
if (!index_eeprom()) {
AP_HAL::panic("Failed to index eeprom");
}
gcs().send_text(MAV_SEVERITY_DEBUG, "Fence Indexed OK");
#endif
void_index();
// this may be completely bogus total. If we are storing an
// advanced fence then the old protocol which relies on this value
// will error off if the GCS tries to fetch points. This number
// should be correct for a "compatible" fence, however.
uint16_t new_total = 0;
if (total_vertex_count < 3) {
new_total = 0;
} else {
new_total = total_vertex_count+2;
}
_total.set_and_save(new_total);
return true;
}
#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT
bool AC_PolyFence_loader::get_return_point(Vector2l &ret)
{
if (!check_indexed()) {
return false;
}
const FenceIndex *rp = find_first_fence(AC_PolyFenceType::RETURN_POINT);
if (rp != nullptr) {
uint16_t read_offset = rp->storage_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<count; i++) {
Vector2l new_loc;
if (!read_latlon_from_storage(offset, new_loc)) {
return false;
}
if (new_loc.is_zero()) {
return false;
}
if (new_loc.x < min_loc.x) {
min_loc.x = new_loc.x;
}
if (new_loc.y < min_loc.y) {
min_loc.y = new_loc.y;
}
if (new_loc.x > max_loc.x) {
max_loc.x = new_loc.x;
}
if (new_loc.y > max_loc.y) {
max_loc.y = new_loc.y;
}
}
// Potential for int32_t overflow when longitudes are beyond [-107, 107].
// As a result, the calculated return point's longitude is calculated using overflowed figure.
// Dividing first before adding avoids the potential overflow.
ret.x = (min_loc.x / 2) + (max_loc.x / 2);
ret.y = (min_loc.y / 2) + (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; i<inclusion_fence->count; 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::CIRCLE_INCLUSION_INT:
case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
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;
}
}
Reference in New Issue View Git Blame Copy Permalink