From 50c14e62cfba510480a96d51e6016c11268fdba0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Nov 2021 13:47:16 +1100 Subject: [PATCH] AC_Fence: fixed fence count for old upload when uploading a fence that is smaller than an old fence we were not correctly setting the inclusion fence size. --- libraries/AC_Fence/AC_PolyFence_loader.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index 6cf664d41a..a45abe9f89 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -1535,11 +1535,18 @@ void AC_PolyFence_loader::handle_msg_fence_point(GCS_MAVLINK &link, const mavlin 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. + /* this is the fence closing point. We use this to set the vertex + count of the inclusion fence + */ + const FenceIndex *inclusion_fence = get_or_create_include_fence(); + if (inclusion_fence == nullptr) { + return; + } + // write type and length + fence_storage.write_uint8(inclusion_fence->storage_offset, uint8_t(AC_PolyFenceType::POLYGON_INCLUSION)); + fence_storage.write_uint8(inclusion_fence->storage_offset+1, packet.idx-1); + // and write end of storage marker + fence_storage.write_uint8(inclusion_fence->storage_offset+2+(packet.idx-1)*8, uint8_t(AC_PolyFenceType::END_OF_STORAGE)); } else { const FenceIndex *inclusion_fence = get_or_create_include_fence(); if (inclusion_fence == nullptr) {