AC_Fence: remove old polygon fence conversion code

This commit is contained in:
Peter Barker 2023-08-10 07:29:03 +10:00 committed by Andrew Tridgell
parent 4936b443ed
commit 4d3e7bd3da
2 changed files with 1 additions and 92 deletions

View File

@ -190,21 +190,6 @@ bool AC_PolyFence_loader::read_latlon_from_storage(uint16_t &read_offset, Vector
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
{
Location loc;
@ -313,70 +298,6 @@ bool AC_PolyFence_loader::format()
return write_eos_to_storage(offset);
}
bool AC_PolyFence_loader::convert_to_new_storage()
{
// sanity check total
_total.set(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;
@ -541,7 +462,7 @@ void AC_PolyFence_loader::scan_eeprom_index_fences(const AC_PolyFenceType type,
bool AC_PolyFence_loader::index_eeprom()
{
if (!formatted()) {
if (!convert_to_new_storage()) {
if (!format()) {
return false;
}
}

View File

@ -350,18 +350,6 @@ private:
Vector2f *&next_storage_point,
Vector2l *&next_storage_point_lla) WARN_IF_UNUSED;
/*
* Upgrade functions - attempt to keep user's fences when
* upgrading to new firmware
*/
// convert_to_new_storage - will attempt to change a pre-existing
// stored fence to the new storage format (so people don't lose
// their fences when upgrading)
bool convert_to_new_storage() WARN_IF_UNUSED;
// load boundary point from eeprom, returns true on successful load
bool load_point_from_eeprom(uint16_t i, Vector2l& point) const WARN_IF_UNUSED;
#if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT
/*
* FENCE_POINT protocol compatability