Line data Source code
1 : #include "AC_PolyFence_loader.h"
2 :
3 : #if AP_FENCE_ENABLED
4 :
5 : #include <AP_Vehicle/AP_Vehicle_Type.h>
6 : #include <AP_BoardConfig/AP_BoardConfig.h>
7 :
8 : #ifndef AC_FENCE_DUMMY_METHODS_ENABLED
9 : #define AC_FENCE_DUMMY_METHODS_ENABLED (!(APM_BUILD_TYPE(APM_BUILD_Rover) | APM_BUILD_COPTER_OR_HELI | APM_BUILD_TYPE(APM_BUILD_ArduPlane) | APM_BUILD_TYPE(APM_BUILD_ArduSub) | (AP_FENCE_ENABLED == 1)))
10 : #endif
11 :
12 : #if !AC_FENCE_DUMMY_METHODS_ENABLED
13 :
14 : #include <AP_AHRS/AP_AHRS.h>
15 : #include <GCS_MAVLink/GCS.h>
16 : #include <AP_Logger/AP_Logger.h>
17 : #include <AC_Fence/AC_Fence.h>
18 :
19 : #include <stdio.h>
20 :
21 : #define POLYFENCE_LOADER_DEBUGGING 0
22 :
23 : #if POLYFENCE_LOADER_DEBUGGING
24 : #define Debug(fmt, args ...) do { GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); } while (0)
25 : #else
26 : #define Debug(fmt, args ...)
27 : #endif
28 :
29 : extern const AP_HAL::HAL& hal;
30 :
31 : static StorageAccess fence_storage(StorageManager::StorageFence);
32 :
33 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
34 : #define AC_FENCE_SDCARD_FILENAME "APM/fence.stg"
35 : #else
36 : #define AC_FENCE_SDCARD_FILENAME "fence.stg"
37 : #endif
38 :
39 2054 : void AC_PolyFence_loader::init()
40 : {
41 : #if AP_SDCARD_STORAGE_ENABLED
42 : // check for extra storage on microsd
43 2054 : const auto *bc = AP::boardConfig();
44 2054 : if (bc != nullptr) {
45 2054 : const auto size_kb = bc->get_sdcard_fence_kb();
46 2054 : if (size_kb > 0) {
47 1 : _failed_sdcard_storage = !fence_storage.attach_file(AC_FENCE_SDCARD_FILENAME, size_kb);
48 1 : if (_failed_sdcard_storage) {
49 : // wipe fence if storage not available, but don't
50 : // save. This allows sdcard error to be fixed and
51 : // reboot
52 0 : _total.set(0);
53 : }
54 : }
55 : }
56 : #endif
57 2054 : if (!check_indexed()) {
58 : // tell the user, perhaps?
59 : }
60 2054 : _old_total = _total;
61 2054 : }
62 :
63 2786 : bool AC_PolyFence_loader::find_index_for_seq(const uint16_t seq, const FenceIndex *&entry, uint16_t &i) const
64 : {
65 2786 : if (_index == nullptr) {
66 0 : return false;
67 : }
68 :
69 2786 : if (seq > _eeprom_item_count) {
70 0 : return false;
71 : }
72 :
73 2786 : i = 0;
74 9212 : for (uint16_t j=0; j<_num_fences; j++) {
75 9212 : entry = &_index[j];
76 9212 : if (seq < i + entry->count) {
77 2786 : return true;
78 : }
79 6426 : i += entry->count;
80 : }
81 0 : return false;
82 : }
83 :
84 2786 : 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
85 : {
86 2786 : if (_index == nullptr) {
87 0 : return false;
88 : }
89 :
90 2786 : uint16_t i = 0;
91 2786 : const FenceIndex *entry = nullptr;
92 2786 : if (!find_index_for_seq(seq, entry, i)) {
93 0 : return false;
94 : }
95 :
96 2786 : if (entry == nullptr) {
97 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
98 0 : return false;
99 : }
100 :
101 2786 : const uint16_t delta = seq - i;
102 :
103 2786 : offset = entry->storage_offset;
104 2786 : type = entry->type;
105 2786 : offset++; // skip over type
106 2786 : switch (type) {
107 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
108 53 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
109 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
110 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
111 : case AC_PolyFenceType::CIRCLE_INCLUSION:
112 : case AC_PolyFenceType::CIRCLE_EXCLUSION:
113 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
114 53 : if (delta != 0) {
115 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
116 0 : return false;
117 : }
118 53 : break;
119 2732 : case AC_PolyFenceType::POLYGON_INCLUSION:
120 : case AC_PolyFenceType::POLYGON_EXCLUSION:
121 2732 : vertex_count_offset = offset;
122 2732 : offset += 1; // the count of points in the fence
123 2732 : offset += (delta * 8);
124 2732 : break;
125 1 : case AC_PolyFenceType::RETURN_POINT:
126 1 : if (delta != 0) {
127 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
128 0 : return false;
129 : }
130 1 : break;
131 0 : case AC_PolyFenceType::END_OF_STORAGE:
132 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
133 0 : return false;
134 : }
135 2786 : return true;
136 : }
137 :
138 2786 : bool AC_PolyFence_loader::get_item(const uint16_t seq, AC_PolyFenceItem &item)
139 : {
140 2786 : if (!check_indexed()) {
141 0 : return false;
142 : }
143 :
144 2786 : uint16_t vertex_count_offset = 0; // initialised to make compiler happy
145 : uint16_t offset;
146 : AC_PolyFenceType type;
147 2786 : if (!find_storage_offset_for_seq(seq, offset, type, vertex_count_offset)) {
148 0 : return false;
149 : }
150 :
151 2786 : item.type = type;
152 :
153 2786 : switch (type) {
154 35 : case AC_PolyFenceType::CIRCLE_INCLUSION:
155 : case AC_PolyFenceType::CIRCLE_EXCLUSION:
156 35 : if (!read_latlon_from_storage(offset, item.loc)) {
157 0 : return false;
158 : }
159 35 : item.radius = fence_storage.read_float(offset);
160 35 : break;
161 18 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION: {
162 18 : item.radius = fence_storage.read_float(offset);
163 18 : break;
164 : }
165 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
166 0 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
167 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
168 0 : if (!read_latlon_from_storage(offset, item.loc)) {
169 0 : return false;
170 : }
171 0 : item.radius = fence_storage.read_uint32(offset);
172 : // magically change int item into a float item:
173 0 : if (type == AC_PolyFenceType::CIRCLE_INCLUSION_INT) {
174 0 : item.type = AC_PolyFenceType::CIRCLE_INCLUSION;
175 : } else {
176 0 : item.type = AC_PolyFenceType::CIRCLE_EXCLUSION;
177 : }
178 0 : break;
179 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
180 2732 : case AC_PolyFenceType::POLYGON_INCLUSION:
181 : case AC_PolyFenceType::POLYGON_EXCLUSION:
182 2732 : if (!read_latlon_from_storage(offset, item.loc)) {
183 0 : return false;
184 : }
185 2732 : item.vertex_count = fence_storage.read_uint8(vertex_count_offset);
186 2732 : break;
187 1 : case AC_PolyFenceType::RETURN_POINT:
188 1 : if (!read_latlon_from_storage(offset, item.loc)) {
189 0 : return false;
190 : }
191 1 : break;
192 0 : case AC_PolyFenceType::END_OF_STORAGE:
193 : // read end-of-storage when I should never do so
194 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
195 0 : return false;
196 : }
197 2786 : return true;
198 : }
199 :
200 2367 : bool AC_PolyFence_loader::write_type_to_storage(uint16_t &offset, const AC_PolyFenceType type)
201 : {
202 2367 : fence_storage.write_uint8(offset, (uint8_t)type);
203 2367 : offset++;
204 2367 : return true;
205 : }
206 :
207 950 : bool AC_PolyFence_loader::write_latlon_to_storage(uint16_t &offset, const Vector2l &latlon)
208 : {
209 950 : fence_storage.write_uint32(offset, latlon.x);
210 950 : offset += 4;
211 950 : fence_storage.write_uint32(offset, latlon.y);
212 950 : offset += 4;
213 950 : return true;
214 : }
215 :
216 4465 : bool AC_PolyFence_loader::read_latlon_from_storage(uint16_t &read_offset, Vector2l &ret) const
217 : {
218 4465 : ret.x = fence_storage.read_uint32(read_offset);
219 4465 : read_offset += 4;
220 4465 : ret.y = fence_storage.read_uint32(read_offset);
221 4465 : read_offset += 4;
222 4465 : return true;
223 : }
224 :
225 43919 : bool AC_PolyFence_loader::breached() const
226 : {
227 43919 : Location loc;
228 43919 : if (!AP::ahrs().get_location(loc)) {
229 2899 : return false;
230 : }
231 :
232 41020 : return breached(loc);
233 : }
234 :
235 : // check if a position (expressed as lat/lng) is within the boundary
236 : // returns true if location is outside the boundary
237 3465310 : bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_fence, Vector2f& fence_direction) const
238 : {
239 3465310 : if (!loaded() || total_fence_count() == 0) {
240 2730 : return false;
241 : }
242 :
243 3462580 : Vector2f scaled_pos;
244 3462580 : Vector2l pos { loc.lat, loc.lng };
245 3462580 : if (!scale_latlon_from_origin(loaded_origin, pos, scaled_pos)) {
246 0 : return false;
247 : }
248 :
249 3462580 : const uint16_t num_inclusion = _num_loaded_circle_inclusion_boundaries + _num_loaded_inclusion_boundaries;
250 3462580 : uint16_t num_inclusion_outside = 0;
251 3462580 : distance_outside_fence = -FLT_MAX;
252 :
253 : // check we are inside each inclusion zone:
254 6911541 : for (uint8_t i=0; i<_num_loaded_inclusion_boundaries; i++) {
255 3448961 : const InclusionBoundary &boundary = _loaded_inclusion_boundary[i];
256 3448961 : bool valid_distance = Polygon_closest_distance_point(boundary.points, boundary.count, scaled_pos, fence_direction);
257 3448961 : float distance = fence_direction.length() * 0.01f; // convert back to meters
258 3448961 : if (Polygon_outside(pos, boundary.points_lla, boundary.count)) {
259 2336974 : num_inclusion_outside++;
260 2336974 : if (valid_distance) {
261 2336974 : if (is_positive(distance_outside_fence)) {
262 0 : distance_outside_fence = MIN(distance_outside_fence, distance);
263 : } else {
264 2336974 : distance_outside_fence = distance;
265 : }
266 : }
267 1111987 : } else if (valid_distance) {
268 1111987 : distance_outside_fence = MAX(distance_outside_fence, -distance);
269 : }
270 : }
271 :
272 : // check we are outside each exclusion zone:
273 3467727 : for (uint8_t i=0; i<_num_loaded_exclusion_boundaries; i++) {
274 5735 : const ExclusionBoundary &boundary = _loaded_exclusion_boundary[i];
275 5735 : bool valid_distance = Polygon_closest_distance_point(boundary.points, boundary.count, scaled_pos, fence_direction);
276 5735 : float distance = fence_direction.length() * 0.01f; // convert back to meters
277 5735 : if (!Polygon_outside(pos, boundary.points_lla, boundary.count)) {
278 588 : if (valid_distance) {
279 588 : distance_outside_fence = distance;
280 : } else {
281 0 : distance_outside_fence = 0.0f;
282 : }
283 588 : return true;
284 5147 : } else if (valid_distance) {
285 5147 : distance_outside_fence = MAX(distance_outside_fence, -distance);
286 : }
287 : }
288 :
289 3472105 : for (uint8_t i=0; i<_num_loaded_circle_exclusion_boundaries; i++) {
290 10494 : const ExclusionCircle &circle = _loaded_circle_exclusion_boundary[i];
291 10494 : Location circle_center;
292 10494 : circle_center.lat = circle.point.x;
293 10494 : circle_center.lng = circle.point.y;
294 10494 : const float diff_cm = loc.get_distance(circle_center)*100.0f;
295 10494 : distance_outside_fence = MAX(distance_outside_fence, circle.radius - diff_cm*0.01f);
296 10494 : if (diff_cm < circle.radius * 100.0f) {
297 381 : return true;
298 : }
299 : }
300 :
301 3477623 : for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) {
302 16012 : const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i];
303 16012 : Location circle_center;
304 16012 : if (circle.is_home_centered()) {
305 13864 : circle_center = AP::ahrs().get_home();
306 : } else {
307 2148 : circle_center.lat = circle.point.x;
308 2148 : circle_center.lng = circle.point.y;
309 : }
310 :
311 16012 : const float diff_cm = loc.get_distance(circle_center)*100.0f;
312 16012 : distance_outside_fence = MAX(distance_outside_fence, diff_cm*0.01f - circle.radius);
313 16012 : if (diff_cm > circle.radius * 100.0f) {
314 1643 : num_inclusion_outside++;
315 : }
316 : }
317 :
318 3461611 : if (AC_Fence::option_enabled(AC_Fence::OPTIONS::INCLUSION_UNION, _options)) {
319 : // using union of inclusion areas, we are outside the fence if
320 : // there is at least one inclusion areas and we are outside
321 : // all of them
322 0 : if (num_inclusion > 0 && num_inclusion == num_inclusion_outside) {
323 0 : return true;
324 : }
325 : } else {
326 : // using intersection of inclusion areas. We are outside if we
327 : // are outside any of them
328 3461611 : if (num_inclusion_outside > 0) {
329 2338522 : return true;
330 : }
331 : }
332 :
333 : // if no value was found, reset to 0
334 1123089 : if (is_equal(distance_outside_fence, -FLT_MAX)) {
335 0 : distance_outside_fence = 0.0f;
336 : }
337 :
338 : // no fence breached
339 1123089 : return false;
340 : }
341 :
342 8236 : bool AC_PolyFence_loader::formatted() const
343 : {
344 16181 : return (fence_storage.read_uint8(0) == new_fence_storage_magic &&
345 15890 : fence_storage.read_uint8(1) == 0 &&
346 24126 : fence_storage.read_uint8(2) == 0 &&
347 16181 : fence_storage.read_uint8(3) == 0);
348 : }
349 :
350 1008 : uint16_t AC_PolyFence_loader::max_items() const
351 : {
352 : // this is 84 items on PixHawk
353 1008 : return fence_storage.size() / sizeof(Vector2l);
354 : }
355 :
356 1292 : bool AC_PolyFence_loader::format()
357 : {
358 1292 : uint16_t offset = 0;
359 1292 : fence_storage.write_uint32(offset, 0);
360 1292 : fence_storage.write_uint8(offset, new_fence_storage_magic);
361 1292 : offset += 4;
362 1292 : void_index();
363 1292 : _eeprom_fence_count = 0;
364 1292 : _eeprom_item_count = 0;
365 1292 : return write_eos_to_storage(offset);
366 : }
367 :
368 3464266 : bool AC_PolyFence_loader::scale_latlon_from_origin(const Location &origin, const Vector2l &point, Vector2f &pos_cm) const
369 : {
370 3464266 : Location tmp_loc;
371 3464266 : tmp_loc.lat = point.x;
372 3464266 : tmp_loc.lng = point.y;
373 3464266 : pos_cm = origin.get_distance_NE(tmp_loc) * 100.0f;
374 3464266 : return true;
375 : }
376 :
377 63 : 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)
378 : {
379 1732 : for (uint8_t i=0; i<vertex_count; i++) {
380 : // read from storage to lat/lon
381 1669 : if (!read_latlon_from_storage(read_offset, *next_storage_point_lla)) {
382 0 : return false;
383 : }
384 : // convert lat/lon to position in cm from origin
385 1669 : if (!scale_latlon_from_origin(origin, *next_storage_point_lla, *next_storage_point)) {
386 0 : return false;
387 : }
388 :
389 1669 : next_storage_point_lla++;
390 1669 : next_storage_point++;
391 : }
392 63 : return true;
393 : }
394 :
395 4180 : bool AC_PolyFence_loader::scan_eeprom(scan_fn_t scan_fn)
396 : {
397 4180 : uint16_t read_offset = 0; // skipping reserved first 4 bytes
398 4180 : if (!formatted()) {
399 0 : return false;
400 : }
401 4180 : read_offset += 4;
402 4180 : bool all_done = false;
403 8752 : while (!all_done) {
404 4572 : if (read_offset > fence_storage.size()) {
405 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
406 0 : AP_HAL::panic("did not find end-of-storage-marker before running out of space");
407 : #endif
408 : return false;
409 : }
410 4572 : const AC_PolyFenceType type = (AC_PolyFenceType)fence_storage.read_uint8(read_offset);
411 : // validate what we've just pulled back from storage:
412 4572 : switch (type) {
413 4572 : case AC_PolyFenceType::END_OF_STORAGE:
414 : case AC_PolyFenceType::POLYGON_INCLUSION:
415 : case AC_PolyFenceType::POLYGON_EXCLUSION:
416 : case AC_PolyFenceType::CIRCLE_INCLUSION:
417 : case AC_PolyFenceType::CIRCLE_EXCLUSION:
418 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
419 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
420 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
421 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
422 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
423 : case AC_PolyFenceType::RETURN_POINT:
424 4572 : break;
425 0 : default:
426 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
427 0 : AP_HAL::panic("Fence corrupt (offset=%u)", read_offset);
428 : #endif
429 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence corrupt");
430 : return false;
431 : }
432 :
433 4572 : scan_fn(type, read_offset);
434 4572 : read_offset++;
435 4572 : switch (type) {
436 4180 : case AC_PolyFenceType::END_OF_STORAGE:
437 4180 : _eos_offset = read_offset-1;
438 4180 : all_done = true;
439 4180 : break;
440 272 : case AC_PolyFenceType::POLYGON_INCLUSION:
441 : case AC_PolyFenceType::POLYGON_EXCLUSION: {
442 272 : const uint8_t vertex_count = fence_storage.read_uint8(read_offset);
443 272 : read_offset += 1; // for the count we just read
444 272 : read_offset += vertex_count*8;
445 272 : break;
446 : }
447 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
448 80 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
449 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
450 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
451 : case AC_PolyFenceType::CIRCLE_INCLUSION:
452 : case AC_PolyFenceType::CIRCLE_EXCLUSION: {
453 80 : read_offset += 8; // for latlon
454 80 : read_offset += 4; // for radius
455 80 : break;
456 : }
457 36 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
458 : // Home circle only has a radius
459 36 : read_offset += 4; // for radius
460 36 : break;
461 4 : case AC_PolyFenceType::RETURN_POINT:
462 4 : read_offset += 8; // for latlon
463 4 : break;
464 : }
465 : }
466 4180 : return true;
467 : }
468 :
469 : // note read_offset here isn't const and ALSO is not a reference
470 4252 : void AC_PolyFence_loader::scan_eeprom_count_fences(const AC_PolyFenceType type, uint16_t read_offset)
471 : {
472 4252 : if (type == AC_PolyFenceType::END_OF_STORAGE) {
473 4056 : return;
474 : }
475 196 : _eeprom_fence_count++;
476 196 : switch (type) {
477 0 : case AC_PolyFenceType::END_OF_STORAGE:
478 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
479 0 : break;
480 136 : case AC_PolyFenceType::POLYGON_EXCLUSION:
481 : case AC_PolyFenceType::POLYGON_INCLUSION: {
482 136 : const uint8_t vertex_count = fence_storage.read_uint8(read_offset+1); // skip type
483 136 : _eeprom_item_count += vertex_count;
484 136 : break;
485 : }
486 60 : case AC_PolyFenceType::CIRCLE_INCLUSION:
487 : case AC_PolyFenceType::CIRCLE_EXCLUSION:
488 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
489 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
490 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
491 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
492 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
493 : case AC_PolyFenceType::RETURN_POINT:
494 60 : _eeprom_item_count++;
495 60 : break;
496 : }
497 : }
498 :
499 4056 : bool AC_PolyFence_loader::count_eeprom_fences()
500 : {
501 4056 : _eeprom_fence_count = 0;
502 4056 : _eeprom_item_count = 0;
503 4056 : const bool ret = scan_eeprom(FUNCTOR_BIND_MEMBER(&AC_PolyFence_loader::scan_eeprom_count_fences, void, const AC_PolyFenceType, uint16_t));
504 4056 : return ret;
505 : }
506 :
507 320 : void AC_PolyFence_loader::scan_eeprom_index_fences(const AC_PolyFenceType type, uint16_t read_offset)
508 : {
509 320 : if (_index == nullptr) {
510 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
511 0 : return;
512 : }
513 320 : if (type == AC_PolyFenceType::END_OF_STORAGE) {
514 124 : return;
515 : }
516 196 : FenceIndex &index = _index[_num_fences++];
517 196 : index.type = type;
518 196 : index.storage_offset = read_offset;
519 196 : switch (type) {
520 0 : case AC_PolyFenceType::END_OF_STORAGE:
521 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
522 0 : break;
523 136 : case AC_PolyFenceType::POLYGON_EXCLUSION:
524 : case AC_PolyFenceType::POLYGON_INCLUSION: {
525 136 : const uint8_t vertex_count = fence_storage.read_uint8(read_offset+1);
526 136 : index.count = vertex_count;
527 136 : break;
528 : }
529 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
530 60 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
531 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
532 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
533 : case AC_PolyFenceType::CIRCLE_INCLUSION:
534 : case AC_PolyFenceType::CIRCLE_EXCLUSION:
535 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
536 : case AC_PolyFenceType::RETURN_POINT:
537 60 : index.count = 1;
538 60 : break;
539 : }
540 : }
541 :
542 4056 : bool AC_PolyFence_loader::index_eeprom()
543 : {
544 4056 : if (!formatted()) {
545 291 : if (!format()) {
546 0 : return false;
547 : }
548 : }
549 :
550 4056 : if (!count_eeprom_fences()) {
551 0 : return false;
552 : }
553 :
554 4056 : void_index();
555 :
556 4056 : if (_eeprom_fence_count == 0) {
557 3932 : _num_fences = 0;
558 3932 : _load_attempted = false;
559 3932 : return true;
560 : }
561 :
562 : Debug("Fence: Allocating %u bytes for index",
563 : (unsigned)(_eeprom_fence_count*sizeof(FenceIndex)));
564 124 : _index = NEW_NOTHROW FenceIndex[_eeprom_fence_count];
565 124 : if (_index == nullptr) {
566 0 : return false;
567 : }
568 :
569 124 : _num_fences = 0;
570 124 : if (!scan_eeprom(FUNCTOR_BIND_MEMBER(&AC_PolyFence_loader::scan_eeprom_index_fences, void, const AC_PolyFenceType, uint16_t))) {
571 0 : void_index();
572 0 : return false;
573 : }
574 :
575 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
576 124 : if (_num_fences != _eeprom_fence_count) {
577 0 : AP_HAL::panic("indexed fences not equal to eeprom fences");
578 : }
579 : #endif
580 :
581 124 : _load_attempted = false;
582 :
583 124 : return true;
584 : }
585 :
586 1491641 : bool AC_PolyFence_loader::check_indexed()
587 : {
588 1491641 : if (!_index_attempted) {
589 3055 : _indexed = index_eeprom();
590 3055 : _index_attempted = true;
591 : }
592 1491641 : return _indexed;
593 : }
594 :
595 1358 : void AC_PolyFence_loader::unload()
596 : {
597 1358 : delete[] _loaded_offsets_from_origin;
598 1358 : _loaded_offsets_from_origin = nullptr;
599 :
600 1358 : delete[] _loaded_points_lla;
601 1358 : _loaded_points_lla = nullptr;
602 :
603 1358 : delete[] _loaded_inclusion_boundary;
604 1358 : _loaded_inclusion_boundary = nullptr;
605 1358 : _num_loaded_inclusion_boundaries = 0;
606 :
607 1358 : delete[] _loaded_exclusion_boundary;
608 1358 : _loaded_exclusion_boundary = nullptr;
609 1358 : _num_loaded_exclusion_boundaries = 0;
610 :
611 1358 : delete[] _loaded_circle_inclusion_boundary;
612 1358 : _loaded_circle_inclusion_boundary = nullptr;
613 1358 : _num_loaded_circle_inclusion_boundaries = 0;
614 :
615 1358 : delete[] _loaded_circle_exclusion_boundary;
616 1358 : _loaded_circle_exclusion_boundary = nullptr;
617 1358 : _num_loaded_circle_exclusion_boundaries = 0;
618 :
619 1358 : _loaded_return_point = nullptr;
620 1358 : _loaded_return_point_lla = nullptr;
621 1358 : _load_time_ms = 0;
622 1358 : }
623 :
624 : // return the number of fences of type type in the index:
625 371 : uint16_t AC_PolyFence_loader::index_fence_count(const AC_PolyFenceType type)
626 : {
627 371 : uint16_t ret = 0;
628 980 : for (uint8_t i=0; i<_eeprom_fence_count; i++) {
629 609 : const FenceIndex &index = _index[i];
630 609 : if (index.type == type) {
631 86 : ret++;
632 : }
633 : }
634 371 : return ret;
635 : }
636 :
637 53 : uint16_t AC_PolyFence_loader::sum_of_polygon_point_counts_and_returnpoint()
638 : {
639 53 : uint16_t ret = 0;
640 140 : for (uint8_t i=0; i<_eeprom_fence_count; i++) {
641 87 : const FenceIndex &index = _index[i];
642 87 : switch (index.type) {
643 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
644 23 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
645 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
646 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
647 : case AC_PolyFenceType::CIRCLE_INCLUSION:
648 : case AC_PolyFenceType::CIRCLE_EXCLUSION:
649 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
650 23 : break;
651 1 : case AC_PolyFenceType::RETURN_POINT:
652 1 : ret += 1;
653 1 : break;
654 63 : case AC_PolyFenceType::POLYGON_INCLUSION:
655 : case AC_PolyFenceType::POLYGON_EXCLUSION:
656 63 : ret += index.count;
657 63 : break;
658 0 : case AC_PolyFenceType::END_OF_STORAGE:
659 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
660 0 : break;
661 : }
662 : }
663 53 : return ret;
664 : }
665 :
666 1486795 : bool AC_PolyFence_loader::load_from_storage()
667 : {
668 1486795 : if (!check_indexed()) {
669 0 : return false;
670 : }
671 :
672 1486795 : if (_load_attempted) {
673 1299247 : return _load_time_ms != 0;
674 : }
675 :
676 187548 : if (!AP::ahrs().get_origin(loaded_origin)) {
677 : // Debug("fence load requires origin");
678 186190 : return false;
679 : }
680 :
681 : // find indexes of each fence:
682 1358 : if (!get_loaded_fence_semaphore().take_nonblocking()) {
683 0 : return false;
684 : }
685 :
686 1358 : _load_attempted = true;
687 :
688 1358 : unload();
689 :
690 1358 : if (_eeprom_item_count == 0) {
691 1305 : get_loaded_fence_semaphore().give();
692 1305 : _load_time_ms = AP_HAL::millis();
693 1305 : return true;
694 : }
695 :
696 : { // allocate array to hold offsets-from-origin
697 53 : const uint16_t count = sum_of_polygon_point_counts_and_returnpoint();
698 : Debug("Fence: Allocating %u bytes for points",
699 : (unsigned)(count * sizeof(Vector2f)));
700 1723 : _loaded_offsets_from_origin = NEW_NOTHROW Vector2f[count];
701 1723 : _loaded_points_lla = NEW_NOTHROW Vector2l[count];
702 53 : if (_loaded_offsets_from_origin == nullptr || _loaded_points_lla == nullptr) {
703 0 : unload();
704 0 : get_loaded_fence_semaphore().give();
705 0 : return false;
706 : }
707 : }
708 :
709 : // FIXME: find some way of factoring out all of these allocation routines.
710 :
711 : { // allocate storage for inclusion polyfences:
712 53 : const auto count = index_fence_count(AC_PolyFenceType::POLYGON_INCLUSION);
713 : Debug("Fence: Allocating %u bytes for inc. fences",
714 : (unsigned)(count * sizeof(InclusionBoundary)));
715 53 : _loaded_inclusion_boundary = NEW_NOTHROW InclusionBoundary[count];
716 53 : if (_loaded_inclusion_boundary == nullptr) {
717 0 : unload();
718 0 : get_loaded_fence_semaphore().give();
719 0 : return false;
720 : }
721 : }
722 :
723 : { // allocate storage for exclusion polyfences:
724 53 : const auto count = index_fence_count(AC_PolyFenceType::POLYGON_EXCLUSION);
725 : Debug("Fence: Allocating %u bytes for exc. fences",
726 : (unsigned)(count * sizeof(ExclusionBoundary)));
727 53 : _loaded_exclusion_boundary = NEW_NOTHROW ExclusionBoundary[count];
728 53 : if (_loaded_exclusion_boundary == nullptr) {
729 0 : unload();
730 0 : get_loaded_fence_semaphore().give();
731 0 : return false;
732 : }
733 : }
734 :
735 : { // allocate storage for circular inclusion fences:
736 53 : uint32_t count = index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION);
737 53 : count += index_fence_count(AC_PolyFenceType::HOME_CIRCLE_INCLUSION);
738 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
739 53 : count += index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION_INT);
740 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
741 : Debug("Fence: Allocating %u bytes for circ. inc. fences",
742 : (unsigned)(count * sizeof(InclusionCircle)));
743 66 : _loaded_circle_inclusion_boundary = NEW_NOTHROW InclusionCircle[count];
744 53 : if (_loaded_circle_inclusion_boundary == nullptr) {
745 0 : unload();
746 0 : get_loaded_fence_semaphore().give();
747 0 : return false;
748 : }
749 : }
750 :
751 : { // allocate storage for circular exclusion fences:
752 53 : uint32_t count = index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION);
753 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
754 53 : count += index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION_INT);
755 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
756 : Debug("Fence: Allocating %u bytes for circ. exc. fences",
757 : (unsigned)(count * sizeof(ExclusionCircle)));
758 63 : _loaded_circle_exclusion_boundary = NEW_NOTHROW ExclusionCircle[count];
759 53 : if (_loaded_circle_exclusion_boundary == nullptr) {
760 0 : unload();
761 0 : get_loaded_fence_semaphore().give();
762 0 : return false;
763 : }
764 : }
765 :
766 53 : Vector2f *next_storage_point = _loaded_offsets_from_origin;
767 53 : Vector2l *next_storage_point_lla = _loaded_points_lla;
768 :
769 : // use index to load fences from eeprom
770 53 : bool storage_valid = true;
771 140 : for (uint8_t i=0; i<_eeprom_fence_count; i++) {
772 87 : if (!storage_valid) {
773 0 : break;
774 : }
775 87 : const FenceIndex &index = _index[i];
776 87 : uint16_t storage_offset = index.storage_offset;
777 87 : storage_offset += 1; // skip type
778 87 : switch (index.type) {
779 0 : case AC_PolyFenceType::END_OF_STORAGE:
780 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
781 0 : AP_HAL::panic("indexed end of storage found");
782 : #endif
783 : storage_valid = false;
784 : break;
785 36 : case AC_PolyFenceType::POLYGON_INCLUSION: {
786 : // FIXME: consider factoring this with the EXCLUSION case
787 36 : InclusionBoundary &boundary = _loaded_inclusion_boundary[_num_loaded_inclusion_boundaries];
788 36 : boundary.points = next_storage_point;
789 36 : boundary.points_lla = next_storage_point_lla;
790 36 : boundary.count = index.count;
791 36 : if (index.count < 3) {
792 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count %u", index.count);
793 0 : storage_valid = false;
794 0 : break;
795 : }
796 36 : storage_offset += 1; // skip vertex count
797 36 : if (!read_polygon_from_storage(loaded_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) {
798 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
799 0 : storage_valid = false;
800 0 : break;
801 : }
802 36 : _num_loaded_inclusion_boundaries++;
803 36 : break;
804 : }
805 27 : case AC_PolyFenceType::POLYGON_EXCLUSION: {
806 27 : ExclusionBoundary &boundary = _loaded_exclusion_boundary[_num_loaded_exclusion_boundaries];
807 27 : boundary.points = next_storage_point;
808 27 : boundary.points_lla = next_storage_point_lla;
809 27 : boundary.count = index.count;
810 27 : if (index.count < 3) {
811 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count %u", index.count);
812 0 : storage_valid = false;
813 0 : break;
814 : }
815 27 : storage_offset += 1; // skip vertex count
816 27 : if (!read_polygon_from_storage(loaded_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) {
817 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
818 0 : storage_valid = false;
819 0 : break;
820 : }
821 27 : _num_loaded_exclusion_boundaries++;
822 27 : break;
823 : }
824 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
825 10 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
826 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
827 : case AC_PolyFenceType::CIRCLE_EXCLUSION: {
828 10 : ExclusionCircle &circle = _loaded_circle_exclusion_boundary[_num_loaded_circle_exclusion_boundaries];
829 10 : if (!read_latlon_from_storage(storage_offset, circle.point)) {
830 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
831 0 : storage_valid = false;
832 0 : break;
833 : }
834 10 : if (!scale_latlon_from_origin(loaded_origin, circle.point, circle.pos_cm)) {
835 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
836 0 : storage_valid = false;
837 0 : break;
838 : }
839 : // now read the radius
840 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
841 10 : if (index.type == AC_PolyFenceType::CIRCLE_EXCLUSION_INT) {
842 0 : circle.radius = fence_storage.read_uint32(storage_offset);
843 : } else
844 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
845 : {
846 10 : circle.radius = fence_storage.read_float(storage_offset);
847 : }
848 10 : if (!is_positive(circle.radius)) {
849 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
850 0 : storage_valid = false;
851 0 : break;
852 : }
853 10 : _num_loaded_circle_exclusion_boundaries++;
854 10 : break;
855 : }
856 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
857 6 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
858 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
859 : case AC_PolyFenceType::CIRCLE_INCLUSION: {
860 6 : InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries];
861 6 : if (!read_latlon_from_storage(storage_offset, circle.point)) {
862 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
863 0 : storage_valid = false;
864 0 : break;
865 : }
866 6 : if (!scale_latlon_from_origin(loaded_origin, circle.point, circle.pos_cm)){
867 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
868 0 : storage_valid = false;
869 0 : break;
870 : }
871 : // now read the radius
872 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
873 6 : if (index.type == AC_PolyFenceType::CIRCLE_INCLUSION_INT) {
874 0 : circle.radius = fence_storage.read_uint32(storage_offset);
875 : } else
876 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
877 : {
878 6 : circle.radius = fence_storage.read_float(storage_offset);
879 : }
880 6 : if (!is_positive(circle.radius)) {
881 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
882 0 : storage_valid = false;
883 0 : break;
884 : }
885 6 : _num_loaded_circle_inclusion_boundaries++;
886 6 : break;
887 : }
888 7 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION: {
889 7 : if (!AP::ahrs().home_is_set()) {
890 : // technically we could look at this when we go to use
891 : // the fence, but that's a lot of places to get
892 : // correct!
893 0 : return false;
894 : }
895 7 : InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries];
896 7 : circle.set_home_centered();
897 : // now read the radius
898 7 : circle.radius = fence_storage.read_float(storage_offset);
899 7 : if (!is_positive(circle.radius)) {
900 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
901 0 : storage_valid = false;
902 0 : break;
903 : }
904 7 : _num_loaded_circle_inclusion_boundaries++;
905 7 : break;
906 : }
907 1 : case AC_PolyFenceType::RETURN_POINT:
908 1 : if (_loaded_return_point != nullptr) {
909 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
910 0 : storage_valid = false;
911 0 : break;
912 : }
913 1 : _loaded_return_point = next_storage_point;
914 1 : if (_loaded_return_point_lla != nullptr) {
915 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
916 0 : storage_valid = false;
917 0 : break;
918 : }
919 1 : _loaded_return_point_lla = next_storage_point_lla;
920 : // Read the point from storage
921 1 : if (!read_latlon_from_storage(storage_offset, *next_storage_point_lla)) {
922 0 : storage_valid = false;
923 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
924 0 : break;
925 : }
926 1 : if (!scale_latlon_from_origin(loaded_origin, *next_storage_point_lla, *next_storage_point)) {
927 0 : storage_valid = false;
928 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
929 0 : break;
930 : }
931 1 : next_storage_point++;
932 1 : next_storage_point_lla++;
933 1 : break;
934 : }
935 : }
936 :
937 53 : if (!storage_valid) {
938 0 : unload();
939 0 : get_loaded_fence_semaphore().give();
940 0 : return false;
941 : }
942 :
943 53 : _load_time_ms = AP_HAL::millis();
944 :
945 53 : get_loaded_fence_semaphore().give();
946 53 : return true;
947 : }
948 :
949 : /// returns pointer to array of exclusion polygon points and num_points is filled in with the number of points in the polygon
950 : /// points are offsets in cm from EKF origin in NE frame
951 14495 : Vector2f* AC_PolyFence_loader::get_exclusion_polygon(uint16_t index, uint16_t &num_points) const
952 : {
953 14495 : if (index >= _num_loaded_exclusion_boundaries) {
954 0 : num_points = 0;
955 0 : return nullptr;
956 : }
957 14495 : const ExclusionBoundary &boundary = _loaded_exclusion_boundary[index];
958 14495 : num_points = boundary.count;
959 :
960 14495 : return boundary.points;
961 : }
962 :
963 : /// returns pointer to array of inclusion polygon points and num_points is filled in with the number of points in the polygon
964 : /// points are offsets in cm from EKF origin in NE frame
965 146027 : Vector2f* AC_PolyFence_loader::get_inclusion_polygon(uint16_t index, uint16_t &num_points) const
966 : {
967 146027 : if (index >= _num_loaded_inclusion_boundaries) {
968 0 : num_points = 0;
969 0 : return nullptr;
970 : }
971 146027 : const InclusionBoundary &boundary = _loaded_inclusion_boundary[index];
972 146027 : num_points = boundary.count;
973 :
974 146027 : return boundary.points;
975 : }
976 :
977 : /// returns the specified exclusion circle
978 : /// circle center offsets in cm from EKF origin in NE frame, radius is in meters
979 56620 : bool AC_PolyFence_loader::get_exclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const
980 : {
981 56620 : if (index >= _num_loaded_circle_exclusion_boundaries) {
982 0 : return false;
983 : }
984 56620 : center_pos_cm = _loaded_circle_exclusion_boundary[index].pos_cm;
985 56620 : radius = _loaded_circle_exclusion_boundary[index].radius;
986 56620 : return true;
987 : }
988 :
989 : /// returns the specified inclusion circle
990 : /// circle centre offsets in cm from EKF origin in NE frame, radius is in meters
991 50722 : bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const
992 : {
993 50722 : if (index >= _num_loaded_circle_inclusion_boundaries) {
994 0 : return false;
995 : }
996 50722 : const auto &circle = _loaded_circle_inclusion_boundary[index];
997 50722 : if (circle.is_home_centered()) {
998 : // Express home as an offset from EKF origin in the NE frame, matching
999 : // the coordinate space callers (AC_Avoid, OA) expect. Queried live so
1000 : // that avoidance tracks home if it is relocated during flight.
1001 50275 : Location origin;
1002 50275 : if (!AP::ahrs().get_origin(origin)) {
1003 0 : return false;
1004 : }
1005 50275 : center_pos_cm = origin.get_distance_NE(AP::ahrs().get_home()) * 100.0f;
1006 : } else {
1007 447 : center_pos_cm = circle.pos_cm;
1008 : }
1009 50722 : radius = circle.radius;
1010 50722 : return true;
1011 : }
1012 :
1013 874 : bool AC_PolyFence_loader::check_inclusion_circle_margin(float margin) const
1014 : {
1015 : // check circular includes
1016 1094 : for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) {
1017 220 : const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i];
1018 220 : if (circle.radius < margin) {
1019 : // circle radius should never be less than margin
1020 0 : return false;
1021 : }
1022 : }
1023 874 : return true;
1024 : }
1025 :
1026 1001 : bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint16_t count) const
1027 : {
1028 : // validate the fence items...
1029 1001 : AC_PolyFenceType expecting_type = AC_PolyFenceType::END_OF_STORAGE;
1030 1001 : uint16_t expected_type_count = 0;
1031 1001 : uint16_t orig_expected_type_count = 0;
1032 1001 : bool seen_return_point = false;
1033 :
1034 1958 : for (uint16_t i=0; i<count; i++) {
1035 957 : bool validate_latlon = false;
1036 :
1037 957 : switch (new_items[i].type) {
1038 0 : case AC_PolyFenceType::END_OF_STORAGE:
1039 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1040 0 : AP_HAL::panic("passed in an END_OF_STORAGE");
1041 : #endif
1042 : return false;
1043 :
1044 934 : case AC_PolyFenceType::POLYGON_INCLUSION:
1045 : case AC_PolyFenceType::POLYGON_EXCLUSION:
1046 934 : if (new_items[i].vertex_count < 3) {
1047 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Invalid vertex count (%u)", new_items[i].vertex_count);
1048 0 : return false;
1049 : }
1050 934 : if (expected_type_count == 0) {
1051 51 : expected_type_count = new_items[i].vertex_count;
1052 51 : orig_expected_type_count = expected_type_count;
1053 51 : expecting_type = new_items[i].type;
1054 : } else {
1055 883 : if (new_items[i].type != expecting_type) {
1056 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect vertex type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
1057 0 : return false;
1058 883 : } else if (new_items[i].vertex_count != orig_expected_type_count) {
1059 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unexpected vertex count want=%u got=%u", orig_expected_type_count, new_items[i].vertex_count);
1060 0 : return false;
1061 : }
1062 : }
1063 934 : expected_type_count--;
1064 934 : validate_latlon = true;
1065 934 : break;
1066 :
1067 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
1068 0 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
1069 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
1070 : // should never have AC_PolyFenceItems of these types
1071 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1072 0 : return false;
1073 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
1074 15 : case AC_PolyFenceType::CIRCLE_INCLUSION:
1075 : case AC_PolyFenceType::CIRCLE_EXCLUSION:
1076 15 : if (expected_type_count) {
1077 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
1078 0 : return false;
1079 : }
1080 15 : if (!is_positive(new_items[i].radius)) {
1081 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Non-positive circle radius");
1082 0 : return false;
1083 : }
1084 15 : validate_latlon = true;
1085 15 : break;
1086 7 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
1087 7 : if (expected_type_count) {
1088 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
1089 0 : return false;
1090 : }
1091 7 : if (!is_positive(new_items[i].radius)) {
1092 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Non-positive circle radius");
1093 0 : return false;
1094 : }
1095 : // Rely on the AHRS home checking.
1096 7 : break;
1097 1 : case AC_PolyFenceType::RETURN_POINT:
1098 1 : if (expected_type_count) {
1099 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
1100 0 : return false;
1101 : }
1102 :
1103 : // spec says only one return point allowed
1104 1 : if (seen_return_point) {
1105 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Multiple return points");
1106 0 : return false;
1107 : }
1108 1 : seen_return_point = true;
1109 1 : validate_latlon = true;
1110 : // TODO: ensure return point is within all fences and
1111 : // outside all exclusion zones
1112 1 : break;
1113 : }
1114 :
1115 957 : if (validate_latlon) {
1116 950 : if (!check_latlng(new_items[i].loc[0], new_items[i].loc[1])) {
1117 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Bad lat or lon");
1118 0 : return false;
1119 : }
1120 : }
1121 : }
1122 :
1123 1001 : if (expected_type_count) {
1124 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Incorrect item count");
1125 0 : return false;
1126 : }
1127 :
1128 1001 : return true;
1129 : }
1130 :
1131 1001 : uint16_t AC_PolyFence_loader::fence_storage_space_required(const AC_PolyFenceItem *new_items, uint16_t count)
1132 : {
1133 1001 : uint16_t ret = 4; // for the format header
1134 1001 : uint16_t i = 0;
1135 1075 : while (i < count) {
1136 74 : ret += 1; // one byte for type
1137 74 : switch (new_items[i].type) {
1138 51 : case AC_PolyFenceType::POLYGON_INCLUSION:
1139 : case AC_PolyFenceType::POLYGON_EXCLUSION:
1140 51 : ret += 1 + 8 * new_items[i].vertex_count; // 1 count, 4 lat, 4 lon for each point
1141 51 : i += new_items[i].vertex_count - 1; // i is incremented down below
1142 51 : break;
1143 0 : case AC_PolyFenceType::END_OF_STORAGE:
1144 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1145 0 : break;
1146 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
1147 0 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
1148 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
1149 : // should never have AC_PolyFenceItems of these types
1150 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1151 0 : break;
1152 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
1153 15 : case AC_PolyFenceType::CIRCLE_INCLUSION:
1154 : case AC_PolyFenceType::CIRCLE_EXCLUSION:
1155 15 : ret += 12; // 4 radius, 4 lat, 4 lon
1156 15 : break;
1157 7 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
1158 7 : ret += 4; // radius
1159 7 : break;
1160 1 : case AC_PolyFenceType::RETURN_POINT:
1161 1 : ret += 8; // 4 lat, 4 lon
1162 1 : break;
1163 : }
1164 74 : i++;
1165 : }
1166 1001 : return ret;
1167 : }
1168 :
1169 1001 : bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_t count)
1170 : {
1171 1001 : if (!validate_fence(new_items, count)) {
1172 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence validation failed");
1173 0 : return false;
1174 : }
1175 :
1176 1001 : if (fence_storage_space_required(new_items, count) > fence_storage.size()) {
1177 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence exceeds storage size");
1178 0 : return false;
1179 : }
1180 :
1181 1001 : if (!format()) {
1182 0 : return false;
1183 : }
1184 :
1185 1001 : uint16_t total_vertex_count = 0;
1186 1001 : uint16_t offset = 4; // skipping magic
1187 1001 : uint8_t vertex_count = 0;
1188 1958 : for (uint16_t i=0; i<count; i++) {
1189 957 : const AC_PolyFenceItem new_item = new_items[i];
1190 957 : switch (new_item.type) {
1191 934 : case AC_PolyFenceType::POLYGON_INCLUSION:
1192 : case AC_PolyFenceType::POLYGON_EXCLUSION:
1193 934 : if (vertex_count == 0) {
1194 : // write out new polygon count
1195 51 : vertex_count = new_item.vertex_count;
1196 51 : total_vertex_count += vertex_count;
1197 51 : if (!write_type_to_storage(offset, new_item.type)) {
1198 0 : return false;
1199 : }
1200 51 : fence_storage.write_uint8(offset, vertex_count);
1201 51 : offset++;
1202 : }
1203 934 : vertex_count--;
1204 934 : if (!write_latlon_to_storage(offset, new_item.loc)) {
1205 0 : return false;
1206 : }
1207 934 : break;
1208 0 : case AC_PolyFenceType::END_OF_STORAGE:
1209 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1210 0 : AP_HAL::panic("asked to store end-of-storage marker");
1211 : #endif
1212 : return false;
1213 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
1214 0 : case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
1215 : case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
1216 : // should never have AC_PolyFenceItems of these types
1217 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1218 0 : return false;
1219 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
1220 15 : case AC_PolyFenceType::CIRCLE_INCLUSION:
1221 : case AC_PolyFenceType::CIRCLE_EXCLUSION: {
1222 15 : total_vertex_count++; // useful to make number of lines in QGC file match FENCE_TOTAL
1223 15 : if (!write_type_to_storage(offset, new_item.type)) {
1224 0 : return false;
1225 : }
1226 15 : if (!write_latlon_to_storage(offset, new_item.loc)) {
1227 0 : return false;
1228 : }
1229 15 : fence_storage.write_float(offset, new_item.radius);
1230 15 : offset += 4; // radius is 4 bytes.
1231 15 : break;
1232 : }
1233 7 : case AC_PolyFenceType::HOME_CIRCLE_INCLUSION: {
1234 7 : total_vertex_count++; // useful to make number of lines in QGC file match FENCE_TOTAL
1235 7 : if (!write_type_to_storage(offset, new_item.type)) {
1236 0 : return false;
1237 : }
1238 7 : fence_storage.write_float(offset, new_item.radius);
1239 7 : offset += 4; // radius is 4 bytes.
1240 7 : break;
1241 : }
1242 1 : case AC_PolyFenceType::RETURN_POINT:
1243 1 : if (!write_type_to_storage(offset, new_item.type)) {
1244 0 : return false;
1245 : }
1246 1 : if (!write_latlon_to_storage(offset, new_item.loc)) {
1247 0 : return false;
1248 : }
1249 1 : break;
1250 : }
1251 : }
1252 1001 : if (!write_eos_to_storage(offset)) {
1253 0 : return false;
1254 : }
1255 :
1256 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1257 : // sanity-check the EEPROM in SITL to make sure we can read what
1258 : // we've just written.
1259 1001 : if (!index_eeprom()) {
1260 0 : AP_HAL::panic("Failed to index eeprom");
1261 : }
1262 1001 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Fence Indexed OK");
1263 : #endif
1264 :
1265 : #if HAL_LOGGING_ENABLED
1266 : // start logger logging new fence
1267 1001 : AP::logger().Write_Fence();
1268 : #endif
1269 :
1270 1001 : void_index();
1271 :
1272 : // this may be completely bogus total. If we are storing an
1273 : // advanced fence then the old protocol which relies on this value
1274 : // will error off if the GCS tries to fetch points. This number
1275 : // should be correct for a "compatible" fence, however.
1276 1001 : uint16_t new_total = 0;
1277 1001 : if (total_vertex_count >= 3) {
1278 37 : new_total = total_vertex_count+2;
1279 : }
1280 1001 : _total.set_and_save(new_total);
1281 :
1282 1001 : return true;
1283 : }
1284 :
1285 :
1286 6 : bool AC_PolyFence_loader::get_return_point(Vector2l &ret)
1287 : {
1288 6 : if (!check_indexed()) {
1289 0 : return false;
1290 : }
1291 :
1292 6 : const FenceIndex *rp = find_first_fence(AC_PolyFenceType::RETURN_POINT);
1293 6 : if (rp != nullptr) {
1294 1 : uint16_t read_offset = rp->storage_offset + 1;
1295 1 : return read_latlon_from_storage(read_offset, ret);
1296 : }
1297 :
1298 5 : const FenceIndex *inc = find_first_fence(AC_PolyFenceType::POLYGON_INCLUSION);
1299 5 : if (inc == nullptr) {
1300 3 : return false;
1301 : }
1302 :
1303 : // we found an inclusion fence but not a return point. Calculate
1304 : // and return the centroid. Note that this may not actually be
1305 : // inside all inclusion fences...
1306 2 : uint16_t offset = inc->storage_offset;
1307 2 : if ((AC_PolyFenceType)fence_storage.read_uint8(offset) != AC_PolyFenceType::POLYGON_INCLUSION) {
1308 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1309 0 : AP_HAL::panic("wrong type at offset");
1310 : #endif
1311 : return false;
1312 : }
1313 2 : offset++;
1314 2 : const uint8_t count = fence_storage.read_uint8(offset);
1315 2 : if (count < 3) {
1316 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1317 0 : AP_HAL::panic("invalid count found");
1318 : #endif
1319 : return false;
1320 : }
1321 2 : offset++;
1322 2 : Vector2l min_loc;
1323 2 : if (!read_latlon_from_storage(offset, min_loc)) {
1324 0 : return false;
1325 : }
1326 2 : if (min_loc.is_zero()) {
1327 0 : return false;
1328 : }
1329 2 : Vector2l max_loc = min_loc;
1330 10 : for (uint8_t i=1; i<count; i++) {
1331 8 : Vector2l new_loc;
1332 8 : if (!read_latlon_from_storage(offset, new_loc)) {
1333 0 : return false;
1334 : }
1335 8 : if (new_loc.is_zero()) {
1336 0 : return false;
1337 : }
1338 8 : if (new_loc.x < min_loc.x) {
1339 1 : min_loc.x = new_loc.x;
1340 : }
1341 8 : if (new_loc.y < min_loc.y) {
1342 1 : min_loc.y = new_loc.y;
1343 : }
1344 8 : if (new_loc.x > max_loc.x) {
1345 2 : max_loc.x = new_loc.x;
1346 : }
1347 8 : if (new_loc.y > max_loc.y) {
1348 3 : max_loc.y = new_loc.y;
1349 : }
1350 : }
1351 :
1352 : // Potential for int32_t overflow when longitudes are beyond [-107, 107].
1353 : // As a result, the calculated return point's longitude is calculated using overflowed figure.
1354 : // Dividing first before adding avoids the potential overflow.
1355 2 : ret.x = (min_loc.x / 2) + (max_loc.x / 2);
1356 2 : ret.y = (min_loc.y / 2) + (max_loc.y / 2);
1357 :
1358 2 : return true;
1359 : }
1360 :
1361 11 : AC_PolyFence_loader::FenceIndex *AC_PolyFence_loader::find_first_fence(const AC_PolyFenceType type) const
1362 : {
1363 11 : if (_index == nullptr) {
1364 6 : return nullptr;
1365 : }
1366 7 : for (uint8_t i=0; i<_num_fences; i++) {
1367 5 : if (_index[i].type == type) {
1368 3 : return &_index[i];
1369 : }
1370 : }
1371 2 : return nullptr;
1372 : }
1373 :
1374 2293 : bool AC_PolyFence_loader::write_eos_to_storage(uint16_t &offset)
1375 : {
1376 2293 : if (!write_type_to_storage(offset, AC_PolyFenceType::END_OF_STORAGE)) {
1377 0 : return false;
1378 : }
1379 2293 : _eos_offset = offset - 1; // should point to the marker
1380 2293 : return true;
1381 : }
1382 :
1383 1486795 : void AC_PolyFence_loader::update()
1384 : {
1385 1486795 : if (!load_from_storage()) {
1386 186190 : return;
1387 : }
1388 : }
1389 :
1390 : #else // build type is not appropriate; provide a dummy implementation:
1391 :
1392 32 : void AC_PolyFence_loader::init() {};
1393 :
1394 0 : bool AC_PolyFence_loader::get_item(const uint16_t seq, AC_PolyFenceItem &item) { return false; }
1395 :
1396 0 : Vector2f* AC_PolyFence_loader::get_exclusion_polygon(uint16_t index, uint16_t &num_points) const { return nullptr; }
1397 0 : Vector2f* AC_PolyFence_loader::get_inclusion_polygon(uint16_t index, uint16_t &num_points) const { return nullptr; }
1398 :
1399 0 : bool AC_PolyFence_loader::get_exclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const { return false; }
1400 0 : bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const { return false; }
1401 :
1402 0 : bool AC_PolyFence_loader::breached() const { return false; }
1403 0 : bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_fence, Vector2f& fence_direction) const { return false; }
1404 :
1405 0 : uint16_t AC_PolyFence_loader::max_items() const { return 0; }
1406 :
1407 0 : bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_t count) { return false; }
1408 :
1409 0 : void AC_PolyFence_loader::update() {};
1410 :
1411 : #endif // #if AC_FENCE_DUMMY_METHODS_ENABLED
1412 : #endif // AP_FENCE_ENABLED
|
