Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 : This program is distributed in the hope that it will be useful,
7 : but WITHOUT ANY WARRANTY; without even the implied warranty of
8 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
9 : GNU General Public License for more details.
10 : You should have received a copy of the GNU General Public License
11 : along with this program. If not, see <http://www.gnu.org/licenses/>.
12 : */
13 :
14 : #include "AP_SmartRTL.h"
15 :
16 : #include <AP_AHRS/AP_AHRS.h>
17 : #include <AP_Logger/AP_Logger.h>
18 : #include <GCS_MAVLink/GCS.h>
19 :
20 : extern const AP_HAL::HAL& hal;
21 :
22 : const AP_Param::GroupInfo AP_SmartRTL::var_info[] = {
23 : // @Param: ACCURACY
24 : // @DisplayName: SmartRTL accuracy
25 : // @Description: SmartRTL accuracy. The minimum distance between points.
26 : // @Units: m
27 : // @Range: 0 10
28 : // @User: Advanced
29 : AP_GROUPINFO("ACCURACY", 0, AP_SmartRTL, _accuracy, SMARTRTL_ACCURACY_DEFAULT),
30 :
31 : // @Param: POINTS
32 : // @DisplayName: SmartRTL maximum number of points on path
33 : // @Description: SmartRTL maximum number of points on path. Set to 0 to disable SmartRTL. 100 points consumes about 3k of memory.
34 : // @Range: 0 500
35 : // @User: Advanced
36 : // @RebootRequired: True
37 : AP_GROUPINFO("POINTS", 1, AP_SmartRTL, _points_max, SMARTRTL_POINTS_DEFAULT),
38 :
39 : // @Param: OPTIONS
40 : // @DisplayName: SmartRTL options
41 : // @Description: Bitmask of SmartRTL options.
42 : // @Bitmask: 2:Ignore pilot yaw
43 : // @User: Standard
44 : AP_GROUPINFO("OPTIONS", 2, AP_SmartRTL, _options, 0),
45 :
46 : AP_GROUPEND
47 : };
48 :
49 : /*
50 : * This library is used for the Safe Return-to-Launch feature. The vehicle's
51 : * position (aka "bread crumbs") are stored into an array in memory at
52 : * regular intervals. After a certain number of bread crumbs have been
53 : * stored and space within the array is low, clean-up algorithms are run to
54 : * reduce the total number of points. When Safe-RTL is initiated by the
55 : * vehicle code, a more thorough cleanup runs and the resulting path is fed
56 : * into navigation controller to return the vehicle to home.
57 : *
58 : * The cleanup consists of two parts, pruning and simplification:
59 : *
60 : * 1. Pruning calculates the closest distance between two line segments formed
61 : * by two pairs of sequential points, and then cuts out anything between two
62 : * points when their line segments get close. This algorithm will never
63 : * compare two consecutive line segments. Obviously the segments (p1,p2) and
64 : * (p2,p3) will get very close (they touch), but there would be nothing to
65 : * trim between them.
66 : *
67 : * 2. Simplification uses the Ramer-Douglas-Peucker algorithm. See Wikipedia
68 : * for a more complete description.
69 : *
70 : * The simplification and pruning algorithms run in the background and do not
71 : * alter the path in memory. Two definitions, SMARTRTL_SIMPLIFY_TIME_US and
72 : * SMARTRTL_PRUNING_LOOP_TIME_US are used to limit how long each algorithm will
73 : * be run before they save their state and return.
74 : *
75 : * Both algorithms are "anytime algorithms" meaning they can be interrupted
76 : * before they complete which is helpful when memory is filling up and we just
77 : * need to quickly identify a handful of points which can be deleted.
78 : *
79 : * Once the algorithms have completed the simplify.complete and
80 : * prune.complete flags are set to true. The "thorough cleanup" procedure,
81 : * which is run as the vehicle initiates the SmartRTL flight mode, waits for
82 : * these flags to become true. This can force the vehicle to pause for a few
83 : * seconds before initiating the return journey.
84 : */
85 :
86 1286 : AP_SmartRTL::AP_SmartRTL(bool example_mode) :
87 1286 : _example_mode(example_mode)
88 : {
89 1286 : AP_Param::setup_object_defaults(this, var_info);
90 1286 : _simplify.bitmask.setall();
91 1286 : }
92 :
93 : // Return true if SmartRTL is enabled
94 89049 : bool AP_SmartRTL::enabled() const
95 : {
96 89049 : return _points_max > 0 && is_positive(_accuracy);
97 : }
98 :
99 : // initialise safe rtl including setting up background processes
100 1281 : void AP_SmartRTL::init()
101 : {
102 : // protect against repeated call to init
103 1281 : if (_path != nullptr) {
104 0 : return;
105 : }
106 :
107 : // constrain the path length, in case the user decided to make the path unreasonably long.
108 1281 : _points_max.set(constrain_int16(_points_max, 0, SMARTRTL_POINTS_MAX));
109 :
110 : // check if user has disabled SmartRTL
111 1281 : if (!enabled()) {
112 0 : return;
113 : }
114 :
115 : // allocate arrays
116 1281 : _path = (Vector3f*)calloc(_points_max, sizeof(Vector3f));
117 :
118 1281 : _prune.loops_max = _points_max * SMARTRTL_PRUNING_LOOP_BUFFER_LEN_MULT;
119 1281 : _prune.loops = (prune_loop_t*)calloc(_prune.loops_max, sizeof(prune_loop_t));
120 :
121 1281 : _simplify.stack_max = _points_max * SMARTRTL_SIMPLIFY_STACK_LEN_MULT;
122 1281 : _simplify.stack = (simplify_start_finish_t*)calloc(_simplify.stack_max, sizeof(simplify_start_finish_t));
123 :
124 : // check if memory allocation failed
125 1281 : if (_path == nullptr || _prune.loops == nullptr || _simplify.stack == nullptr) {
126 0 : log_action(Action::DEACTIVATED_INIT_FAILED);
127 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SmartRTL deactivated: init failed");
128 0 : free(_path);
129 0 : free(_prune.loops);
130 0 : free(_simplify.stack);
131 0 : return;
132 : }
133 :
134 1281 : _path_points_max = _points_max;
135 :
136 : // when running the example sketch, we want the cleanup tasks to run when we tell them to, not in the background (so that they can be timed.)
137 1281 : if (!_example_mode){
138 : // register background cleanup to run in IO thread
139 1281 : hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_SmartRTL::run_background_cleanup, void));
140 : }
141 : }
142 :
143 : // returns number of points on the path
144 74 : uint16_t AP_SmartRTL::get_num_points() const
145 : {
146 74 : return _path_points_count;
147 : }
148 :
149 : // get next point on the path to home, returns true on success
150 49 : bool AP_SmartRTL::pop_point(Vector3p& point)
151 : {
152 : // check we are active
153 49 : if (!_active) {
154 0 : return false;
155 : }
156 :
157 : // get semaphore
158 49 : if (!_path_sem.take_nonblocking()) {
159 0 : log_action(Action::POP_FAILED_NO_SEMAPHORE);
160 0 : return false;
161 : }
162 :
163 : // check we have another point
164 49 : if (_path_points_count == 0) {
165 0 : _path_sem.give();
166 0 : return false;
167 : }
168 :
169 : // return last point and remove from path
170 49 : point = _path[--_path_points_count].topostype();
171 :
172 : // record count of last point popped
173 49 : _path_points_completed_limit = _path_points_count;
174 :
175 49 : _path_sem.give();
176 49 : return true;
177 : }
178 :
179 : // peek at next point on the path without removing it form the path. Returns true on success
180 39 : bool AP_SmartRTL::peek_point(Vector3p& point)
181 : {
182 : // check we are active
183 39 : if (!_active) {
184 0 : return false;
185 : }
186 :
187 : // get semaphore
188 39 : if (!_path_sem.take_nonblocking()) {
189 0 : log_action(Action::PEEK_FAILED_NO_SEMAPHORE);
190 0 : return false;
191 : }
192 :
193 : // check we have another point
194 39 : if (_path_points_count == 0) {
195 2 : _path_sem.give();
196 2 : return false;
197 : }
198 :
199 : // return last point
200 37 : point = _path[_path_points_count-1].topostype();
201 :
202 37 : _path_sem.give();
203 37 : return true;
204 : }
205 :
206 : // clear return path and set home location. This should be called as part of the arming procedure
207 15352 : void AP_SmartRTL::set_home(bool position_ok)
208 : {
209 15352 : Vector3p current_pos;
210 15352 : position_ok &= AP::ahrs().get_relative_position_NED_origin(current_pos);
211 15352 : set_home(position_ok, current_pos);
212 15352 : }
213 :
214 15352 : void AP_SmartRTL::set_home(bool position_ok, const Vector3p& current_pos)
215 : {
216 15352 : if (_path == nullptr) {
217 0 : return;
218 : }
219 :
220 : // clear path
221 15352 : _path_points_count = 0;
222 :
223 : // reset simplification and pruning. These functions access members that should normally only
224 : // be touched by the background thread but it will not be running because active should be false
225 15352 : reset_simplification();
226 15352 : reset_pruning();
227 :
228 : // don't continue if no position at take-off
229 15352 : if (!position_ok) {
230 13626 : return;
231 : }
232 :
233 : // save current position as first point in path
234 1726 : if (!add_point(current_pos)) {
235 0 : return;
236 : }
237 :
238 : // successfully added point and reset path
239 1726 : const uint32_t now = AP_HAL::millis();
240 1726 : _last_good_position_ms = now;
241 1726 : _last_position_save_ms = now;
242 1726 : _active = true;
243 1726 : _home_saved = true;
244 : }
245 :
246 : // call this at 3hz (or higher) regardless of what mode the vehicle is in
247 267333 : void AP_SmartRTL::update(bool position_ok, bool save_position)
248 : {
249 : // try to save home if not already saved
250 267333 : if (position_ok && !_home_saved) {
251 13561 : set_home(true);
252 : }
253 :
254 267333 : if (!_active || !save_position) {
255 95702 : return;
256 : }
257 :
258 171631 : Vector3p current_pos;
259 171631 : position_ok &= AP::ahrs().get_relative_position_NED_origin(current_pos);
260 171631 : update(position_ok, current_pos);
261 : }
262 :
263 171631 : void AP_SmartRTL::update(bool position_ok, const Vector3p& current_pos)
264 : {
265 171631 : if (!_active) {
266 0 : return;
267 : }
268 :
269 171631 : if (position_ok) {
270 171149 : const uint32_t now = AP_HAL::millis();
271 171149 : _last_good_position_ms = now;
272 : // add the point
273 171149 : if (add_point(current_pos)) {
274 171149 : _last_position_save_ms = now;
275 0 : } else if (AP_HAL::millis() - _last_position_save_ms > SMARTRTL_TIMEOUT) {
276 : // deactivate after timeout due to failure to save points to path (most likely due to buffer filling up)
277 0 : deactivate(Action::DEACTIVATED_PATH_FULL_TIMEOUT, "buffer full");
278 : }
279 : } else {
280 : // check for timeout due to bad position
281 482 : if (AP_HAL::millis() - _last_good_position_ms > SMARTRTL_TIMEOUT) {
282 10 : deactivate(Action::DEACTIVATED_BAD_POSITION_TIMEOUT, "bad position");
283 10 : return;
284 : }
285 : }
286 : }
287 :
288 : // request thorough cleanup including simplification, pruning and removal of all unnecessary points
289 : // returns true if the thorough cleanup was completed, false if it has not yet completed
290 : // this method should be called repeatedly until it returns true before initiating the return journey
291 34 : bool AP_SmartRTL::request_thorough_cleanup(ThoroughCleanupType clean_type)
292 : {
293 : // this should never happen but just in case
294 34 : if (!_active) {
295 0 : return false;
296 : }
297 :
298 : // request thorough cleanup
299 34 : if (_thorough_clean_request_ms == 0) {
300 17 : _thorough_clean_request_ms = AP_HAL::millis();
301 17 : if (clean_type != THOROUGH_CLEAN_DEFAULT) {
302 0 : _thorough_clean_type = clean_type;
303 : }
304 17 : return false;
305 : }
306 :
307 : // check if background thread has completed request
308 17 : if (_thorough_clean_complete_ms == _thorough_clean_request_ms) {
309 17 : _thorough_clean_request_ms = 0;
310 17 : return true;
311 : }
312 :
313 0 : return false;
314 : }
315 :
316 : // cancel request for thorough cleanup
317 14 : void AP_SmartRTL::cancel_request_for_thorough_cleanup()
318 : {
319 14 : _thorough_clean_request_ms = 0;
320 14 : }
321 :
322 : //
323 : // Private methods
324 : //
325 :
326 : // add point to end of path (if necessary), returns true on success
327 172879 : bool AP_SmartRTL::add_point(const Vector3p& point)
328 : {
329 : // get semaphore
330 172879 : if (!_path_sem.take_nonblocking()) {
331 0 : log_action(Action::ADD_FAILED_NO_SEMAPHORE, point.tofloat());
332 0 : return false;
333 : }
334 :
335 : // check if we have traveled far enough
336 172879 : if (_path_points_count > 0) {
337 171152 : const Vector3p& last_pos = _path[_path_points_count-1].topostype();
338 171152 : if (last_pos.distance_squared(point) < sq(_accuracy.get())) {
339 139234 : _path_sem.give();
340 139234 : return true;
341 : }
342 : }
343 :
344 : // check we have space in the path
345 33645 : if (_path_points_count >= _path_points_max) {
346 0 : _path_sem.give();
347 0 : log_action(Action::ADD_FAILED_PATH_FULL, point.tofloat());
348 0 : return false;
349 : }
350 :
351 : // add point to path
352 33645 : _path[_path_points_count++] = point.tofloat();
353 33645 : log_action(Action::POINT_ADD, point.tofloat());
354 :
355 33645 : _path_sem.give();
356 33645 : return true;
357 : }
358 :
359 : // run background cleanup - should be run regularly from the IO thread
360 146860393 : void AP_SmartRTL::run_background_cleanup()
361 : {
362 146860393 : if (!_active) {
363 36375660 : return;
364 : }
365 :
366 : // get semaphore
367 110484733 : if (!_path_sem.take_nonblocking()) {
368 0 : return;
369 : }
370 : // local copy of _path_points_count and _path_points_completed_limit
371 110484733 : const uint16_t path_points_count = _path_points_count;
372 110484733 : const uint16_t path_points_completed_limit = _path_points_completed_limit;
373 110484733 : _path_points_completed_limit = SMARTRTL_POINTS_MAX;
374 110484733 : _path_sem.give();
375 :
376 : // check if thorough cleanup is required
377 110484733 : if (_thorough_clean_request_ms > 0) {
378 : // check if we have already completed the request
379 138 : if (_thorough_clean_complete_ms != _thorough_clean_request_ms) {
380 56 : if (thorough_cleanup(path_points_count, _thorough_clean_type)) {
381 : // record completion
382 17 : _thorough_clean_complete_ms = _thorough_clean_request_ms;
383 : }
384 : }
385 : // we do not perform any further detection or cleanup until the requester acknowledges
386 : // they have what they need by setting _thorough_clean_request_ms back to zero
387 138 : return;
388 : }
389 :
390 : // ensure clean complete time is zero
391 110484595 : _thorough_clean_complete_ms = 0;
392 :
393 : // perform routine cleanup which removes 10 to 50 points if possible
394 110484595 : routine_cleanup(path_points_count, path_points_completed_limit);
395 :
396 : // warn if buffer is about to be filled
397 110484595 : uint32_t now_ms = AP_HAL::millis();
398 110484595 : if ((path_points_count >0) && (path_points_count >= _path_points_max - 9) && (now_ms - _last_low_space_notify_ms > 10000)) {
399 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SmartRTL Low on space!");
400 0 : _last_low_space_notify_ms = now_ms;
401 : }
402 :
403 : }
404 :
405 : // routine cleanup is called regularly from run_background_cleanup
406 : // simplifies the path after SMARTRTL_CLEANUP_POINT_TRIGGER points (50 points) have been added OR
407 : // SMARTRTL_CLEANUP_POINT_MIN (10 points) have been added and the path has less than SMARTRTL_CLEANUP_START_MARGIN spaces (10 spaces) remaining
408 : // prunes the path if the path has less than SMARTRTL_CLEANUP_START_MARGIN spaces (10 spaces) remaining
409 110484595 : void AP_SmartRTL::routine_cleanup(uint16_t path_points_count, uint16_t path_points_completed_limit)
410 : {
411 : // if simplify is running, let it run to completion
412 110484595 : if (!_simplify.complete) {
413 1661 : detect_simplifications();
414 1661 : return;
415 : }
416 :
417 : // remove simplified from path if required
418 110482934 : if (_simplify.removal_required) {
419 453 : remove_points_by_simplify_bitmask();
420 453 : return;
421 : }
422 :
423 : // if necessary restart detect_pruning up to last point simplified
424 110482481 : if (_prune.complete) {
425 110481274 : restart_pruning_if_new_points();
426 : }
427 : // if pruning is running, let it run to completion
428 110482481 : if (!_prune.complete) {
429 1705 : detect_loops();
430 1705 : return;
431 : }
432 :
433 : // detect path shrinkage and reduce simplify and prune path_points_completed count
434 110480776 : if (_simplify.path_points_completed > path_points_completed_limit) {
435 44 : _simplify.path_points_completed = path_points_completed_limit;
436 : }
437 110480776 : if (_prune.path_points_completed > path_points_completed_limit) {
438 40 : _prune.path_points_completed = path_points_completed_limit;
439 : }
440 :
441 : // calculate the number of points we could simplify
442 110480776 : const uint16_t points_to_simplify = (path_points_count > _simplify.path_points_completed) ? (path_points_count - _simplify.path_points_completed) : 0 ;
443 110480776 : const bool low_on_space = (_path_points_max - path_points_count) <= SMARTRTL_CLEANUP_START_MARGIN;
444 :
445 : // if 50 points can be simplified or we are low on space and at least 10 points can be simplified
446 110480776 : if ((points_to_simplify >= SMARTRTL_CLEANUP_POINT_TRIGGER) || (low_on_space && (points_to_simplify >= SMARTRTL_CLEANUP_POINT_MIN))) {
447 454 : restart_simplification(path_points_count);
448 454 : return;
449 : }
450 :
451 : // we are low on space, prune
452 110480322 : if (low_on_space) {
453 : // remove at least 10 points
454 0 : remove_points_by_loops(SMARTRTL_CLEANUP_POINT_MIN);
455 : }
456 : }
457 :
458 : // thorough cleanup simplifies and prunes all loops. returns true if the cleanup was completed.
459 : // path_points_count is _path_points_count but passed in to avoid having to take the semaphore
460 56 : bool AP_SmartRTL::thorough_cleanup(uint16_t path_points_count, ThoroughCleanupType clean_type)
461 : {
462 56 : if (clean_type != THOROUGH_CLEAN_PRUNE_ONLY) {
463 : // restart simplify if new points have appeared on path
464 56 : if (_simplify.complete) {
465 56 : restart_simplify_if_new_points(path_points_count);
466 : }
467 : // if simplification is not complete, run it
468 56 : if (!_simplify.complete) {
469 16 : detect_simplifications();
470 16 : return false;
471 : }
472 : // remove simplified points from path if required
473 40 : if (_simplify.removal_required) {
474 11 : remove_points_by_simplify_bitmask();
475 11 : return false;
476 : }
477 : }
478 :
479 29 : if (clean_type != THOROUGH_CLEAN_SIMPLIFY_ONLY) {
480 : // if necessary restart detect_pruning up to last point simplified
481 29 : if (_prune.complete) {
482 29 : restart_pruning_if_new_points();
483 : }
484 : // if pruning is not complete, run it
485 29 : if (!_prune.complete) {
486 12 : detect_loops();
487 12 : return false;
488 : }
489 : // remove pruning points
490 17 : if (!remove_points_by_loops(SMARTRTL_POINTS_MAX)) {
491 0 : return false;
492 : }
493 : }
494 :
495 17 : return true;
496 : }
497 :
498 : // Simplifies a 3D path, according to the Ramer-Douglas-Peucker algorithm.
499 : // _simplify.complete is set to true when all simplifications on the path have been identified
500 1677 : void AP_SmartRTL::detect_simplifications()
501 : {
502 : // complete immediately if only one segment
503 1677 : if (_simplify.path_points_count < 3) {
504 1211 : _simplify.complete = true;
505 1211 : return;
506 : }
507 :
508 : // if not complete but also nothing to do, we must be restarting
509 466 : if (_simplify.stack_count == 0) {
510 : // reset to beginning state. add a single element in the array with:
511 : // start = first path point OR the index of the last already-simplified point
512 : // finish = final path point
513 466 : _simplify.stack[0].start = (_simplify.path_points_completed > 0) ? _simplify.path_points_completed - 1 : 0;
514 466 : _simplify.stack[0].finish = _simplify.path_points_count-1;
515 466 : _simplify.stack_count++;
516 : }
517 :
518 466 : const uint32_t start_time_us = AP_HAL::micros();
519 5430 : while (_simplify.stack_count > 0) { // while there is something to do
520 :
521 : // if this method has run for long enough, exit
522 4964 : if (AP_HAL::micros() - start_time_us > SMARTRTL_SIMPLIFY_TIME_US) {
523 0 : return;
524 : }
525 :
526 : // pop last item off the simplify stack
527 4964 : const simplify_start_finish_t tmp = _simplify.stack[--_simplify.stack_count];
528 4964 : const uint16_t start_index = tmp.start;
529 4964 : const uint16_t end_index = tmp.finish;
530 :
531 : // find the point between start and end points that is farthest from the start-end line segment
532 4964 : float max_dist = 0.0f;
533 4964 : uint16_t farthest_point_index = start_index;
534 62717 : for (uint16_t i = start_index + 1; i < end_index; i++) {
535 : // only check points that have not already been flagged for simplification
536 57753 : if (_simplify.bitmask.get(i)) {
537 57753 : const float dist = _path[i].distance_to_segment(_path[start_index], _path[end_index]);
538 57753 : if (dist > max_dist) {
539 23107 : farthest_point_index = i;
540 23107 : max_dist = dist;
541 : }
542 : }
543 : }
544 :
545 : // if the farthest point is more than ACCURACY * 0.5 add two new elements to the _simplification_stack
546 : // so that on the next iteration we will check between start-to-farthestpoint and farthestpoint-to-end
547 4964 : if (max_dist > SMARTRTL_SIMPLIFY_EPSILON) {
548 : // if the to-do list is full, give up on simplifying. This should never happen.
549 2249 : if (_simplify.stack_count >= _simplify.stack_max) {
550 0 : _simplify.complete = true;
551 0 : return;
552 : }
553 2249 : _simplify.stack[_simplify.stack_count++] = simplify_start_finish_t {start_index, farthest_point_index};
554 2249 : _simplify.stack[_simplify.stack_count++] = simplify_start_finish_t {farthest_point_index, end_index};
555 : } else {
556 : // if the farthest point was closer than ACCURACY * 0.5 we can simplify all points between start and end
557 22693 : for (uint16_t i = start_index + 1; i < end_index; i++) {
558 19978 : _simplify.bitmask.clear(i);
559 19978 : _simplify.removal_required = true;
560 : }
561 : }
562 : }
563 466 : _simplify.path_points_completed = _simplify.path_points_count;
564 466 : _simplify.complete = true;
565 : }
566 :
567 : /**
568 : * This method runs for the allotted time, and detects loops in a path. Any detected loops are added to _prune.loops,
569 : * this function does not alter the path in memory. It works by comparing the line segment between any two sequential points
570 : * to the line segment between any other two sequential points. If they get close enough, anything between them could be pruned.
571 : *
572 : * reset_pruning should have been called at least once before this function is called to setup the indexes (_prune.i, etc)
573 : */
574 1717 : void AP_SmartRTL::detect_loops()
575 : {
576 : // if there are less than 4 points (3 segments), mark complete
577 1717 : if (_prune.path_points_count < 4) {
578 1301 : _prune.complete = true;
579 1301 : return;
580 : }
581 :
582 : // capture start time
583 416 : const uint32_t start_time_us = AP_HAL::micros();
584 :
585 : // run for defined amount of time
586 65285 : while (AP_HAL::micros() - start_time_us < SMARTRTL_PRUNING_LOOP_TIME_US) {
587 :
588 : // advance inner loop
589 65285 : _prune.j++;
590 65285 : if (_prune.j > _prune.i - 2) {
591 : // set inner loop back to first point
592 2253 : _prune.j = 1;
593 : // reduce outer loop
594 2253 : _prune.i--;
595 : // complete when outer loop has run out of new points to check
596 2253 : if (_prune.i < 4 || _prune.i < _prune.path_points_completed) {
597 416 : _prune.complete = true;
598 416 : _prune.path_points_completed = _prune.path_points_count;
599 416 : return;
600 : }
601 : }
602 :
603 : // find the closest distance between two line segments and the mid-point
604 64869 : dist_point dp = segment_segment_dist(_path[_prune.i], _path[_prune.i-1], _path[_prune.j-1], _path[_prune.j]);
605 64869 : if (dp.distance < SMARTRTL_PRUNING_DELTA) {
606 : // if there is a loop here, add to loop array
607 667 : if (!add_loop(_prune.j, _prune.i-1, dp.midpoint)) {
608 : // if the buffer is full, stop trying to prune
609 0 : _prune.complete = true;
610 : }
611 : // set inner loop forward to trigger outer loop move to next segment
612 667 : _prune.j = _prune.i;
613 : }
614 : }
615 : }
616 :
617 : // restart simplify if new points have been added to path
618 : // path_points_count is _path_points_count but passed in to avoid having to take the semaphore
619 56 : void AP_SmartRTL::restart_simplify_if_new_points(uint16_t path_points_count)
620 : {
621 : // any difference in the number of points is because of new points being added to path
622 56 : if (_simplify.path_points_count != path_points_count) {
623 16 : restart_simplification(path_points_count);
624 : }
625 56 : }
626 :
627 : // reset pruning if new points have been simplified
628 110481303 : void AP_SmartRTL::restart_pruning_if_new_points()
629 : {
630 : // any difference in the number of points is because of new points being added to path
631 110481303 : if (_prune.path_points_count != _simplify.path_points_completed) {
632 510 : restart_pruning(_simplify.path_points_completed);
633 : }
634 110481303 : }
635 :
636 : // restart simplification algorithm so that it will check new points in the path
637 15822 : void AP_SmartRTL::restart_simplification(uint16_t path_points_count)
638 : {
639 15822 : _simplify.complete = false;
640 15822 : _simplify.removal_required = false;
641 15822 : _simplify.bitmask.setall();
642 15822 : _simplify.stack_count = 0;
643 15822 : _simplify.path_points_count = path_points_count;
644 15822 : }
645 :
646 : // reset simplification algorithm so that it will re-check all points in the path
647 15352 : void AP_SmartRTL::reset_simplification()
648 : {
649 15352 : restart_simplification(0);
650 15352 : _simplify.path_points_completed = 0;
651 15352 : }
652 :
653 : // restart pruning algorithm to check new points that have arrived
654 15862 : void AP_SmartRTL::restart_pruning(uint16_t path_points_count)
655 : {
656 15862 : _prune.complete = false;
657 15862 : _prune.i = (path_points_count > 0) ? path_points_count - 1 : 0;
658 15862 : _prune.j = 0;
659 15862 : _prune.path_points_count = path_points_count;
660 15862 : }
661 :
662 : // reset pruning algorithm so that it will re-check all points in the path
663 15352 : void AP_SmartRTL::reset_pruning()
664 : {
665 15352 : restart_pruning(0);
666 15352 : _prune.loops_count = 0; // clear the loops that we've recorded
667 15352 : _prune.path_points_completed = 0;
668 15352 : }
669 :
670 : // remove all simplify-able points from the path
671 464 : void AP_SmartRTL::remove_points_by_simplify_bitmask()
672 : {
673 : // get semaphore before modifying path
674 464 : if (!_path_sem.take_nonblocking()) {
675 0 : return;
676 : }
677 464 : uint16_t dest = 1;
678 464 : uint16_t removed = 0;
679 28452 : for (uint16_t src = 1; src < _path_points_count; src++) {
680 27988 : if (!_simplify.bitmask.get(src)) {
681 19978 : log_action(Action::POINT_SIMPLIFY, _path[src]);
682 19978 : removed++;
683 : } else {
684 8010 : _path[dest] = _path[src];
685 8010 : dest++;
686 : }
687 : }
688 :
689 : // reduce count of the number of points simplified
690 464 : if (_path_points_count > removed && _simplify.path_points_count > removed) {
691 464 : _path_points_count -= removed;
692 464 : _simplify.path_points_count -= removed;
693 464 : _simplify.path_points_completed = _simplify.path_points_count;
694 : } else {
695 : // this is an error that should never happen so deactivate
696 0 : deactivate(Action::DEACTIVATED_PROGRAM_ERROR, "program error");
697 : }
698 :
699 464 : _path_sem.give();
700 :
701 : // flag point removal is complete
702 464 : _simplify.bitmask.setall();
703 464 : _simplify.removal_required = false;
704 : }
705 :
706 : // remove loops until at least num_point_to_delete have been removed from path
707 : // does not necessarily prune all loops
708 : // returns false if it failed to remove points (because it could not take semaphore)
709 17 : bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove)
710 : {
711 : // exit immediately if no loops to prune
712 17 : if (_prune.loops_count == 0) {
713 17 : return true;
714 : }
715 :
716 : // get semaphore before modifying path
717 0 : if (!_path_sem.take_nonblocking()) {
718 0 : return false;
719 : }
720 :
721 0 : uint16_t removed_points = 0;
722 0 : uint16_t i = _prune.loops_count;
723 0 : while ((i > 0) && (removed_points < num_points_to_remove)) {
724 0 : i--;
725 0 : prune_loop_t loop = _prune.loops[i];
726 :
727 : // midpoint goes into start_index (this is the end point of the first segment)
728 0 : _path[loop.start_index] = loop.midpoint;
729 :
730 : // shift points after the end of the loop down by the number of points in the loop
731 0 : uint16_t loop_num_points_to_remove = loop.end_index - loop.start_index;
732 0 : for (uint16_t dest = loop.start_index + 1; dest < _path_points_count - loop_num_points_to_remove; dest++) {
733 0 : log_action(Action::POINT_PRUNE, _path[dest]);
734 0 : _path[dest] = _path[dest + loop_num_points_to_remove];
735 : }
736 :
737 0 : if (_path_points_count > loop_num_points_to_remove) {
738 0 : _path_points_count -= loop_num_points_to_remove;
739 0 : removed_points += loop_num_points_to_remove;
740 : } else {
741 : // this is an error that should never happen so deactivate
742 0 : deactivate(Action::DEACTIVATED_PROGRAM_ERROR, "program error");
743 0 : _path_sem.give();
744 : // we return true so thorough_cleanup does not get stuck
745 0 : return true;
746 : }
747 :
748 : // fix the indices of any existing prune loops
749 : // we do not check for overlapping loops because add_loops should have caught them
750 0 : for (uint16_t loop_cnt = 0; loop_cnt < i; loop_cnt++) {
751 0 : if (_prune.loops[loop_cnt].start_index >= loop.end_index) {
752 0 : _prune.loops[loop_cnt].start_index -= loop_num_points_to_remove;
753 : }
754 0 : if (_prune.loops[loop_cnt].end_index >= loop.end_index) {
755 0 : _prune.loops[loop_cnt].end_index -= loop_num_points_to_remove;
756 : }
757 : }
758 :
759 : // remove last prune loop from array
760 0 : _prune.loops_count--;
761 : }
762 :
763 0 : _path_sem.give();
764 0 : return true;
765 : }
766 :
767 : // add loop to loops array
768 : // returns true if loop added successfully, false if loop array is full
769 : // checks if loop overlaps with an existing loop, keeps only the longer loop
770 667 : bool AP_SmartRTL::add_loop(uint16_t start_index, uint16_t end_index, const Vector3f& midpoint)
771 : {
772 : // if the buffer is full, return failure
773 667 : if (_prune.loops_count >= _prune.loops_max) {
774 0 : return false;
775 : }
776 :
777 : // sanity check indices
778 667 : if (end_index <= start_index) {
779 0 : return false;
780 : }
781 :
782 : // create new loop structure and calculate length squared of loop
783 667 : prune_loop_t new_loop = {start_index, end_index, midpoint, 0.0f};
784 667 : new_loop.length_squared = midpoint.distance_squared(_path[start_index]) + midpoint.distance_squared(_path[end_index]);
785 14663 : for (uint16_t i = start_index; i < end_index; i++) {
786 13996 : new_loop.length_squared += _path[i].distance_squared(_path[i+1]);
787 : }
788 :
789 : // look for overlapping loops and find their combined length
790 667 : bool overlapping_loops = false;
791 667 : float overlapping_loop_length = 0.0f;
792 2001 : for (uint16_t loop_idx = 0; loop_idx < _prune.loops_count; loop_idx++) {
793 1334 : if (loops_overlap(_prune.loops[loop_idx], new_loop)) {
794 616 : overlapping_loops = true;
795 616 : overlapping_loop_length += _prune.loops[loop_idx].length_squared;
796 : }
797 : }
798 :
799 : // handle overlapping loops
800 667 : if (overlapping_loops) {
801 : // if adding this loop would lengthen the path, discard the new loop but return success
802 576 : if (overlapping_loop_length > new_loop.length_squared) {
803 498 : return true;
804 : }
805 : // remove overlapping loops
806 78 : uint16_t dest_idx = 0;
807 78 : uint16_t removed = 0;
808 218 : for (uint16_t src_idx = 0; src_idx < _prune.loops_count; src_idx++) {
809 140 : if (loops_overlap(_prune.loops[src_idx], new_loop)) {
810 88 : removed++;
811 : } else {
812 52 : _prune.loops[dest_idx] = _prune.loops[src_idx];
813 52 : dest_idx++;
814 : }
815 : }
816 78 : _prune.loops_count -= removed;
817 : }
818 :
819 : // add new loop to _prune.loops array
820 169 : _prune.loops[_prune.loops_count] = new_loop;
821 169 : _prune.loops_count++;
822 169 : return true;
823 : }
824 :
825 : /**
826 : * Returns the closest distance in 3D space between any part of two input segments, defined from p1 to p2 and from p3 to p4.
827 : * Also returns the point which is halfway between
828 : *
829 : * Limitation: This function does not work for parallel lines. In this case, dist_point.distance will be FLT_MAX.
830 : * This does not matter for the path cleanup algorithm because the pruning will still occur fine between the first
831 : * parallel segment and a segment which is directly before or after the second segment.
832 : */
833 64869 : AP_SmartRTL::dist_point AP_SmartRTL::segment_segment_dist(const Vector3f &p1, const Vector3f &p2, const Vector3f &p3, const Vector3f &p4)
834 : {
835 64869 : const Vector3f line1 = p2 - p1;
836 64869 : const Vector3f line2 = p4 - p3;
837 64869 : const Vector3f line_start_diff = p1 - p3; // from the beginning of the second line to the beginning of the first line
838 :
839 : // these don't really have a physical representation. They're only here to break up the longer formulas below.
840 64869 : const float a = line1 * line1;
841 64869 : const float b = line1 * line2;
842 64869 : const float c = line2 * line2;
843 64869 : const float d = line1 * line_start_diff;
844 64869 : const float e = line2 * line_start_diff;
845 :
846 : // the parameter for the position on line1 and line2 which define the closest points.
847 64869 : float t1 = 0.0f;
848 64869 : float t2 = 0.0f;
849 :
850 : // if lines are almost parallel, return a garbage answer. This is irrelevant, since the loop
851 : // could always be pruned start/end of the previous/subsequent line segment
852 64869 : if (is_zero((a*c)-(b*b))) {
853 35 : return {FLT_MAX, Vector3f(0.0f, 0.0f, 0.0f)};
854 : }
855 :
856 64834 : t1 = (b * e - c * d) / (a * c - b * b);
857 64834 : t2 = (a * e - b * d) / (a * c - b * b);
858 :
859 : // restrict both parameters between 0 and 1.
860 64834 : t1 = constrain_float(t1, 0.0f, 1.0f);
861 64834 : t2 = constrain_float(t2, 0.0f, 1.0f);
862 :
863 : // difference between two closest points
864 64834 : const Vector3f dP = line_start_diff + line1 * t1 - line2 * t2;
865 :
866 64834 : const Vector3f midpoint = (p1 + (line1 * t1) + p3 + (line2 * t2)) / 2.0;
867 64834 : return {dP.length(), midpoint};
868 : }
869 :
870 : // de-activate SmartRTL, send warning to GCS and logger
871 10 : void AP_SmartRTL::deactivate(Action action, const char *reason)
872 : {
873 10 : _active = false;
874 10 : log_action(action);
875 10 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SmartRTL deactivated: %s", reason);
876 10 : }
877 :
878 : #if HAL_LOGGING_ENABLED
879 : // logging
880 53633 : void AP_SmartRTL::log_action(Action action, const Vector3f &point) const
881 : {
882 53633 : if (!_example_mode) {
883 53633 : AP::logger().Write_SRTL(_active, _path_points_count, _path_points_max, action, point);
884 : }
885 53633 : }
886 : #endif
887 :
888 : // returns true if the two loops overlap (used within add_loop to determine which loops to keep or throw away)
889 1474 : bool AP_SmartRTL::loops_overlap(const prune_loop_t &loop1, const prune_loop_t &loop2) const
890 : {
891 : // check if loop1 within loop2
892 1474 : if (loop1.start_index >= loop2.start_index && loop1.end_index <= loop2.end_index) {
893 79 : return true;
894 : }
895 : // check if loop2 within loop1
896 1395 : if (loop2.start_index >= loop1.start_index && loop2.end_index <= loop1.end_index) {
897 428 : return true;
898 : }
899 : // check for partial overlap (loop1's start OR end point is within loop2)
900 967 : const bool loop1_start_in_loop2 = (loop1.start_index >= loop2.start_index) && (loop1.start_index <= loop2.end_index);
901 967 : const bool loop1_end_in_loop2 = (loop1.end_index >= loop2.start_index) && (loop1.end_index <= loop2.end_index);
902 967 : if (loop1_start_in_loop2 != loop1_end_in_loop2) {
903 197 : return true;
904 : }
905 : // if we got here, no overlap
906 770 : return false;
907 : }
908 :
909 : // returns true if pilot's yaw input should be used to adjust vehicle's heading
910 141536 : bool AP_SmartRTL::use_pilot_yaw(void) const
911 : {
912 141536 : return (_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0;
913 : }
914 :
|
