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

AC_Fence: provide compatability with bad integer-stored radii

This commit is contained in:
Peter Barker 2021-06-03 11:20:57 +10:00 committed by Peter Barker
parent e3db030d7a
commit 76408c77dd
2 changed files with 94 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
libraries/AC_Fence/AC_PolyFence_loader.cpp
View File

@ -69,6 +69,8 @@ bool AC_PolyFence_loader::find_storage_offset_for_seq(const uint16_t seq, uint16
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) {
@ -118,6 +120,19 @@ bool AC_PolyFence_loader::get_item(const uint16_t seq, AC_PolyFenceItem &item)
}
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)) {
@ -382,6 +397,8 @@ bool AC_PolyFence_loader::scan_eeprom(scan_fn_t scan_fn)
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:
@ -406,6 +423,8 @@ bool AC_PolyFence_loader::scan_eeprom(scan_fn_t scan_fn)
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
@ -439,6 +458,8 @@ void AC_PolyFence_loader::scan_eeprom_count_fences(const AC_PolyFenceType type,
}
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;
@ -475,6 +496,8 @@ void AC_PolyFence_loader::scan_eeprom_index_fences(const AC_PolyFenceType type,
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;
@ -584,6 +607,8 @@ uint16_t AC_PolyFence_loader::sum_of_polygon_point_counts_and_returnpoint()
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;
@ -674,6 +699,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
{ // 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];
@ -686,6 +712,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
{ // 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];
@ -754,6 +781,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
_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)) {
@ -767,8 +795,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
break;
}
// now read the radius
circle.radius = fence_storage.read_uint32(storage_offset);
if (circle.radius <= 0) {
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;
@ -776,6 +808,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
_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)) {
@ -789,8 +822,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
break;
}
// now read the radius
circle.radius = fence_storage.read_float(storage_offset);
if (circle.radius <= 0) {
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;
@ -946,13 +983,18 @@ bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint
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 (new_items[i].radius <= 0) {
if (!is_positive(new_items[i].radius)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Non-positive circle radius");
return false;
}
@ -1008,6 +1050,11 @@ uint16_t AC_PolyFence_loader::fence_storage_space_required(const AC_PolyFenceIte
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
@ -1065,19 +1112,41 @@ bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_
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:
case AC_PolyFenceType::CIRCLE_EXCLUSION: {
total_vertex_count++; // useful to make number of lines in QGC file match FENCE_TOTAL
if (!write_type_to_storage(offset, new_item.type)) {
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
fence_storage.write_float(offset, new_item.radius);
// 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;
@ -1522,6 +1591,8 @@ bool AC_PolyFence_loader::contains_compatible_fence() const
}
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:

20
libraries/AC_Fence/AC_PolyFence_loader.h
View File

@ -7,13 +7,21 @@
#define AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT 1
// CIRCLE_INCLUSION_INT stores the radius an a 32-bit integer in
// metres. This was a bug, and CIRCLE_INCLUSION was created to store
// as a 32-bit float instead. We save as _INT in the case that the
// radius looks like an integer as a backwards-compatability measure.
// For 4.2 we might consider only loading _INT and always saving as
// float, and in 4.3 considering _INT invalid
enum class AC_PolyFenceType {
END_OF_STORAGE = 99,
POLYGON_INCLUSION = 98,
POLYGON_EXCLUSION = 97,
CIRCLE_EXCLUSION = 96,
RETURN_POINT = 95,
CIRCLE_INCLUSION = 94,
END_OF_STORAGE = 99,
POLYGON_INCLUSION = 98,
POLYGON_EXCLUSION = 97,
CIRCLE_EXCLUSION_INT = 96,
RETURN_POINT = 95,
CIRCLE_INCLUSION_INT = 94,
CIRCLE_EXCLUSION = 93,
CIRCLE_INCLUSION = 92,
};
// a FenceItem is just a means of passing data about an item into