Line data Source code
1 : #pragma once
2 :
3 : #include "AC_Fence_config.h"
4 : #include <AP_Math/AP_Math.h>
5 :
6 : // CIRCLE_INCLUSION_INT stores the radius an a 32-bit integer in
7 : // metres. This was a bug, and CIRCLE_INCLUSION was created to store
8 : // as a 32-bit float instead. We save as _INT in the case that the
9 : // radius looks like an integer as a backwards-compatibility measure.
10 : // For 4.2 we might consider only loading _INT and always saving as
11 : // float, and in 4.3 considering _INT invalid
12 :
13 : // CODE_REMOVAL
14 : // ArduPilot 4.7 no longer stores circle radiuses that look like
15 : // integers as integer item types, so any time a fence is saved the
16 : // use of the deprecated types is fixed.
17 : // ArduPilot 4.8 warns if it loads an integer item, warns user to re-upload the fence
18 : // ArduPilot 4.9 warns if it loads an integer item, warns user to re-upload the fence
19 : // ArduPilot 4.10 removes support for them
20 :
21 : enum class AC_PolyFenceType : uint8_t {
22 : END_OF_STORAGE = 99,
23 : POLYGON_INCLUSION = 98,
24 : POLYGON_EXCLUSION = 97,
25 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
26 : CIRCLE_EXCLUSION_INT = 96,
27 : #endif // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
28 : RETURN_POINT = 95,
29 : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
30 : CIRCLE_INCLUSION_INT = 94,
31 : #endif // #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
32 : CIRCLE_EXCLUSION = 93,
33 : CIRCLE_INCLUSION = 92,
34 : HOME_CIRCLE_INCLUSION = 91,
35 : };
36 :
37 : // a FenceItem is just a means of passing data about an item into
38 : // and out of the polyfence loader. It uses a AC_PolyFenceType to
39 : // indicate the item type, assuming each fence type is made up of
40 : // only one sort of item.
41 : // TODO: make this a union (or use subclasses) to save memory
42 : class AC_PolyFenceItem {
43 : public:
44 : AC_PolyFenceType type;
45 : Vector2l loc;
46 : uint8_t vertex_count;
47 : float radius;
48 : };
49 :
50 : #if AP_FENCE_ENABLED
51 :
52 : #include <AP_Common/AP_Common.h>
53 : #include <AP_Common/Location.h>
54 : #include <GCS_MAVLink/GCS_MAVLink.h>
55 :
56 : class AC_PolyFence_loader
57 : {
58 :
59 : public:
60 :
61 2098 : AC_PolyFence_loader(AP_Int8 &total, const AP_Int16 &options) :
62 2098 : _total(total),
63 2098 : _options(options) {}
64 :
65 : /* Do not allow copies */
66 : CLASS_NO_COPY(AC_PolyFence_loader);
67 :
68 : void init();
69 :
70 : // methods primarily for MissionItemProtocol_Fence to use:
71 : // return the total number of points stored
72 8604 : uint16_t num_stored_items() const { return _eeprom_item_count; }
73 : // Read from the persistent storage into an item object
74 : bool get_item(const uint16_t seq, AC_PolyFenceItem &item) WARN_IF_UNUSED;
75 :
76 : ///
77 : /// exclusion polygons
78 : ///
79 : /// returns number of polygon exclusion zones defined
80 15247979 : uint8_t get_exclusion_polygon_count() const {
81 15247979 : return _num_loaded_exclusion_boundaries;
82 : }
83 :
84 : /// returns pointer to array of exclusion polygon points and num_points is filled in with the number of points in the polygon
85 : /// points are offsets in cm from EKF origin in NE frame
86 : Vector2f* get_exclusion_polygon(uint16_t index, uint16_t &num_points) const;
87 :
88 : /// return system time of last update to the exclusion polygon points
89 272 : uint32_t get_exclusion_polygon_update_ms() const {
90 272 : return _load_time_ms;
91 : }
92 :
93 : ///
94 : /// inclusion polygons
95 : ///
96 : /// returns number of polygon inclusion zones defined
97 15248388 : uint8_t get_inclusion_polygon_count() const {
98 15248388 : return _num_loaded_inclusion_boundaries;
99 : }
100 :
101 : /// returns pointer to array of inclusion polygon points and num_points is filled in with the number of points in the polygon
102 : /// points are offsets in cm from EKF origin in NE frame
103 : Vector2f* get_inclusion_polygon(uint16_t index, uint16_t &num_points) const;
104 :
105 : /// return system time of last update to the inclusion polygon points
106 272 : uint32_t get_inclusion_polygon_update_ms() const {
107 272 : return _load_time_ms;
108 : }
109 :
110 : ///
111 : /// exclusion circles
112 : ///
113 : /// returns number of exclusion circles defined
114 17207691 : uint8_t get_exclusion_circle_count() const {
115 17207691 : return _num_loaded_circle_exclusion_boundaries;
116 : }
117 :
118 : /// returns the specified exclusion circle
119 : /// center is offsets in cm from EKF origin in NE frame, radius is in meters
120 : bool get_exclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const;
121 :
122 : /// return system time of last update to the exclusion circles
123 271 : uint32_t get_exclusion_circle_update_ms() const {
124 271 : return _load_time_ms;
125 : }
126 :
127 : ///
128 : /// inclusion circles
129 : ///
130 : /// returns number of inclusion circles defined
131 17207653 : uint8_t get_inclusion_circle_count() const {
132 17207653 : return _num_loaded_circle_inclusion_boundaries;
133 : }
134 :
135 : /// returns the specified inclusion circle
136 : /// center is offsets in cm from EKF origin in NE frame, radius is in meters
137 : bool get_inclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const;
138 :
139 : // false if margin < fence radius
140 : bool check_inclusion_circle_margin(float margin) const;
141 :
142 : ///
143 : /// mavlink
144 : ///
145 : // breached() - returns true if the vehicle has breached any fence
146 : bool breached() const WARN_IF_UNUSED;
147 : // returns true if location is outside the boundary also returns the minimum distance to the fence
148 : bool breached(const Location& loc, float& distance_outside_fence, Vector2f& fence_direction) const WARN_IF_UNUSED;
149 : // breached(Location&) - returns true if location is outside the boundary
150 3429854 : bool breached(const Location& loc) const WARN_IF_UNUSED
151 : {
152 : float distance_outside_fence;
153 3429854 : Vector2f breach_direction;
154 3429854 : return breached(loc, distance_outside_fence, breach_direction);
155 : }
156 :
157 : // returns true if a polygonal include fence could be returned
158 351975 : bool inclusion_boundary_available() const WARN_IF_UNUSED {
159 351975 : return _num_loaded_inclusion_boundaries != 0;
160 : }
161 :
162 : // loaded - returns true if the fences have been loaded from
163 : // storage and are available for use
164 3466323 : bool loaded() const WARN_IF_UNUSED {
165 3466323 : return _load_time_ms != 0;
166 : };
167 :
168 : // maximum number of fence points we can store in eeprom
169 : uint16_t max_items() const;
170 :
171 : // write_fence - validate and write count new_items to permanent storage
172 : bool write_fence(const AC_PolyFenceItem *new_items, uint16_t count) WARN_IF_UNUSED;
173 :
174 : /*
175 : * Loaded Fence functionality
176 : *
177 : * methods and members to do with fences stored in memory. The
178 : * locations are translated into offset-from-origin-in-metres
179 : */
180 :
181 : // load polygon points stored in eeprom into
182 : // _loaded_offsets_from_origin and perform validation. returns
183 : // true if load successfully completed
184 : bool load_from_storage() WARN_IF_UNUSED;
185 :
186 : // allow threads to lock against AHRS update
187 319607 : HAL_Semaphore &get_loaded_fence_semaphore(void) {
188 319607 : return _loaded_fence_sem;
189 : }
190 :
191 : // call @10Hz to check for fence load being valid
192 : void update();
193 :
194 : // get_return_point - returns latitude/longitude of return point.
195 : // This works with storage - the returned vector is absolute
196 : // lat/lon.
197 : bool get_return_point(Vector2l &ret) WARN_IF_UNUSED;
198 :
199 : // return total number of fences - polygons and circles
200 15051287 : uint16_t total_fence_count() const {
201 15051287 : return (get_exclusion_polygon_count() +
202 15051284 : get_inclusion_polygon_count() +
203 15051285 : get_exclusion_circle_count() +
204 15051286 : get_inclusion_circle_count());
205 : }
206 :
207 :
208 : #if AP_SDCARD_STORAGE_ENABLED
209 681 : bool failed_sdcard_storage(void) const {
210 681 : return _failed_sdcard_storage;
211 : }
212 : #endif
213 :
214 : private:
215 : // multi-thread access support
216 : HAL_Semaphore _loaded_fence_sem;
217 :
218 : /*
219 : * Fence storage Index related functions
220 : */
221 : // FenceIndex - a class used to store information about a fence in
222 : // fence storage.
223 : class FenceIndex {
224 : public:
225 : AC_PolyFenceType type;
226 : uint16_t count;
227 : uint16_t storage_offset;
228 : };
229 : // index_fence_count - returns the number of fences of type
230 : // currently in the index
231 : uint16_t index_fence_count(const AC_PolyFenceType type);
232 :
233 : // void_index - free resources for the index, forcing a reindex
234 : // (typically via check_indexed)
235 6349 : void void_index() {
236 6349 : delete[] _index;
237 6349 : _index = nullptr;
238 6349 : _index_attempted = false;
239 6349 : _indexed = false;
240 6349 : }
241 :
242 : // check_indexed - read eeprom and create index if the index does
243 : // not already exist
244 : bool check_indexed() WARN_IF_UNUSED;
245 :
246 : // find_first_fence - return first fence in index of specific type
247 : FenceIndex *find_first_fence(const AC_PolyFenceType type) const;
248 :
249 : // find_index_for_seq - returns true if seq is contained within a
250 : // fence. If it is, entry will be the relevant FenceIndex. i
251 : // will be the offset within _loaded_offsets_from_origin where the
252 : // first point in the fence is found
253 : bool find_index_for_seq(const uint16_t seq, const FenceIndex *&entry, uint16_t &i) const WARN_IF_UNUSED;
254 : // find_storage_offset_for_seq - uses the index to return an
255 : // offset into storage for an item
256 : bool find_storage_offset_for_seq(const uint16_t seq, uint16_t &offset, AC_PolyFenceType &type, uint16_t &vertex_count_offset) const WARN_IF_UNUSED;
257 :
258 : uint16_t sum_of_polygon_point_counts_and_returnpoint();
259 :
260 : /*
261 : * storage-related methods - dealing with fence_storage
262 : */
263 :
264 : // new_fence_storage_magic - magic number indicating fence storage
265 : // has been formatted for use by polygon fence storage code.
266 : // FIXME: ensure this is out-of-band for old lat/lon point storage
267 : static const uint8_t new_fence_storage_magic = 235;
268 :
269 : // validate_fence - returns true if new_items look completely valid
270 : bool validate_fence(const AC_PolyFenceItem *new_items, uint16_t count) const WARN_IF_UNUSED;
271 :
272 : // _eos_offset - stores the offset in storage of the
273 : // end-of-storage marker. Used by low-level manipulation code to
274 : // extend storage
275 : uint16_t _eos_offset;
276 :
277 : // formatted - returns true if the fence storage space seems to be
278 : // formatted for new-style fence storage
279 : bool formatted() const WARN_IF_UNUSED;
280 : // format - format the storage space for use by
281 : // the new polyfence code
282 : bool format() WARN_IF_UNUSED;
283 :
284 :
285 : /*
286 : * Loaded Fence functionality
287 : *
288 : * methods and members to do with fences stored in memory. The
289 : * locations are translated into offset-from-origin-in-metres
290 : */
291 :
292 : // remove resources dedicated to the transformed fences - for
293 : // example, in _loaded_offsets_from_origin
294 : void unload();
295 :
296 : // pointer into _loaded_offsets_from_origin where the return point
297 : // can be found:
298 : Vector2f *_loaded_return_point;
299 :
300 : // pointer into _loaded_points_lla where the return point
301 : // can be found:
302 : Vector2l *_loaded_return_point_lla;
303 :
304 : class InclusionBoundary {
305 : public:
306 : Vector2f *points; // pointer into the _loaded_offsets_from_origin array
307 : Vector2l *points_lla; // pointer into the _loaded_points_lla array
308 : uint8_t count; // count of points in the boundary
309 : };
310 : InclusionBoundary *_loaded_inclusion_boundary;
311 :
312 : uint8_t _num_loaded_inclusion_boundaries;
313 :
314 : class ExclusionBoundary {
315 : public:
316 : Vector2f *points; // pointer into the _loaded_offsets_from_origin array
317 : Vector2l *points_lla; // pointer into the _loaded_points_lla_lla array
318 : uint8_t count; // count of points in the boundary
319 : };
320 : ExclusionBoundary *_loaded_exclusion_boundary;
321 :
322 : uint8_t _num_loaded_exclusion_boundaries;
323 :
324 : // _loaded_offsets_from_origin - stores x/y offset-from-origin
325 : // coordinate pairs. Various items store their locations in this
326 : // allocation - the polygon boundaries and the return point, for
327 : // example.
328 : Vector2f *_loaded_offsets_from_origin;
329 : Vector2l *_loaded_points_lla;
330 : Location loaded_origin; // origin at the time the boundary was loaded
331 :
332 : class ExclusionCircle {
333 : public:
334 : Vector2f pos_cm; // vector offset from home in cm
335 : Vector2l point; // lat/lng of zone
336 : float radius;
337 : };
338 : ExclusionCircle *_loaded_circle_exclusion_boundary;
339 :
340 : uint8_t _num_loaded_circle_exclusion_boundaries;
341 :
342 : class InclusionCircle {
343 : public:
344 : Vector2f pos_cm; // vector offset from home in cm
345 : Vector2l point; // scaled lat/lng of zone; INT32_MAX signifies home-centered
346 : float radius;
347 66734 : bool is_home_centered() const WARN_IF_UNUSED {
348 66734 : return point.x == INT32_MAX && point.y == INT32_MAX;
349 : }
350 7 : void set_home_centered() {
351 7 : point.x = INT32_MAX;
352 7 : point.y = INT32_MAX;
353 7 : }
354 : };
355 : InclusionCircle *_loaded_circle_inclusion_boundary;
356 :
357 : uint8_t _num_loaded_circle_inclusion_boundaries;
358 :
359 : // _load_attempted - true if we have attempted to load the fences
360 : // from storage into _loaded_circle_exclusion_boundary,
361 : // _loaded_offsets_from_origin etc etc
362 : bool _load_attempted;
363 :
364 : // _load_time_ms - from millis(), system time when fence load last
365 : // succeeded. Will be zero if fences are not loaded
366 : uint32_t _load_time_ms;
367 :
368 : // scale_latlon_from_origin - given a latitude/longitude
369 : // transforms the point to an offset-from-origin and deposits
370 : // the result into pos_cm.
371 : bool scale_latlon_from_origin(const Location &origin,
372 : const Vector2l &point,
373 : Vector2f &pos_cm) const WARN_IF_UNUSED;
374 :
375 : // read_polygon_from_storage - reads vertex_count
376 : // latitude/longitude points from offset in permanent storage,
377 : // transforms them into an offset-from-origin and deposits the
378 : // results into next_storage_point.
379 : bool read_polygon_from_storage(const Location &origin,
380 : uint16_t &read_offset,
381 : const uint8_t vertex_count,
382 : Vector2f *&next_storage_point,
383 : Vector2l *&next_storage_point_lla) WARN_IF_UNUSED;
384 :
385 : // primitives to write parts of fencepoints out:
386 : bool write_type_to_storage(uint16_t &offset, AC_PolyFenceType type) WARN_IF_UNUSED;
387 : bool write_latlon_to_storage(uint16_t &offset, const Vector2l &latlon) WARN_IF_UNUSED;
388 : bool read_latlon_from_storage(uint16_t &read_offset, Vector2l &latlon) const WARN_IF_UNUSED;
389 :
390 : // methods to write specific types of fencepoint out:
391 : bool write_eos_to_storage(uint16_t &offset);
392 :
393 : // _total - reference to FENCE_TOTAL parameter. This is used
394 : // solely for compatibility with the FENCE_POINT protocol
395 : AP_Int8 &_total;
396 : const AP_Int16 &_options;
397 : uint8_t _old_total;
398 :
399 :
400 : // scan_eeprom - a method that traverses the fence storage area,
401 : // calling the supplied callback for each fence found. If the
402 : // scan fails (for example, the storage is corrupt), then this
403 : // method will return false.
404 : FUNCTOR_TYPEDEF(scan_fn_t, void, const AC_PolyFenceType, uint16_t);
405 : bool scan_eeprom(scan_fn_t scan_fn) WARN_IF_UNUSED;
406 : // scan_eeprom_count_fences - a static function designed to be
407 : // massed to scan_eeprom which counts the number of fences and
408 : // fence items present. The results of this counting appear in _eeprom_fence_count and _eeprom_item_count
409 : void scan_eeprom_count_fences(const AC_PolyFenceType type, uint16_t read_offset);
410 : uint16_t _eeprom_fence_count;
411 : uint16_t _eeprom_item_count;
412 :
413 : // scan_eeprom_index_fences - a static function designed to be
414 : // passed to scan_eeprom. _index must be a pointer to
415 : // memory sufficient to hold information about all fences present
416 : // in storage - so it is expected that scan_eeprom_count_fences
417 : // has been used to count those fences and the allocation already
418 : // made. After this method has been called _index will
419 : // be filled with information about the fences in the fence
420 : // storage - type, item counts and storage offset.
421 : void scan_eeprom_index_fences(const AC_PolyFenceType type, uint16_t read_offset);
422 : // array specifying type of each fence in storage (and a count of
423 : // items in that fence)
424 : FenceIndex *_index;
425 : bool _indexed; // true if indexing successful
426 : bool _index_attempted; // true if we attempted to index the eeprom
427 : // _num_fences - count of the number of fences in _index. This
428 : // should be equal to _eeprom_fence_count
429 : uint16_t _num_fences;
430 :
431 : // count_eeprom_fences - refresh the count of fences in permanent storage
432 : bool count_eeprom_fences() WARN_IF_UNUSED;
433 : // index_eeprom - (re)allocate and fill in _index
434 : bool index_eeprom() WARN_IF_UNUSED;
435 :
436 : uint16_t fence_storage_space_required(const AC_PolyFenceItem *new_items, uint16_t count);
437 :
438 : #if AP_SDCARD_STORAGE_ENABLED
439 : // true if we failed to load SDCard storage on init
440 : bool _failed_sdcard_storage;
441 : #endif
442 : };
443 :
444 : #endif // AP_FENCE_ENABLED
|
