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 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AC_Avoidance_config.h"
17 :
18 : #if AP_OAPATHPLANNER_BENDYRULER_ENABLED
19 :
20 : #include "AP_OABendyRuler.h"
21 : #include <AC_Avoidance/AP_OADatabase.h>
22 : #include <AC_Fence/AC_Fence.h>
23 : #include <AP_AHRS/AP_AHRS.h>
24 : #include <AP_Logger/AP_Logger.h>
25 : #include <AP_Vehicle/AP_Vehicle_Type.h>
26 :
27 : // parameter defaults
28 : const float OA_BENDYRULER_LOOKAHEAD_DEFAULT = 15.0f;
29 : const float OA_BENDYRULER_RATIO_DEFAULT = 1.5f;
30 : const int16_t OA_BENDYRULER_ANGLE_DEFAULT = 75;
31 : const int16_t OA_BENDYRULER_TYPE_DEFAULT = 1;
32 :
33 : const int16_t OA_BENDYRULER_BEARING_INC_XY = 5; // check every 5 degrees around vehicle
34 : const int16_t OA_BENDYRULER_BEARING_INC_VERTICAL = 90;
35 : const float OA_BENDYRULER_LOOKAHEAD_STEP2_RATIO = 1.0f; // step2's lookahead length as a ratio of step1's lookahead length
36 : const float OA_BENDYRULER_LOOKAHEAD_STEP2_MIN = 2.0f; // step2 checks at least this many meters past step1's location
37 : const float OA_BENDYRULER_LOOKAHEAD_PAST_DEST = 2.0f; // lookahead length will be at least this many meters past the destination
38 : const float OA_BENDYRULER_LOW_SPEED_SQUARED = (0.2f * 0.2f); // when ground course is below this speed squared, vehicle's heading will be used
39 :
40 : #define VERTICAL_ENABLED APM_BUILD_COPTER_OR_HELI
41 :
42 : const AP_Param::GroupInfo AP_OABendyRuler::var_info[] = {
43 :
44 : // @Param: LOOKAHEAD
45 : // @DisplayName: Object Avoidance look ahead distance maximum
46 : // @Description: Object Avoidance will look this many meters ahead of vehicle
47 : // @Units: m
48 : // @Range: 1 100
49 : // @Increment: 1
50 : // @User: Standard
51 : AP_GROUPINFO("LOOKAHEAD", 1, AP_OABendyRuler, _lookahead, OA_BENDYRULER_LOOKAHEAD_DEFAULT),
52 :
53 : // @Param: CONT_RATIO
54 : // @DisplayName: Obstacle Avoidance margin ratio for BendyRuler to change bearing significantly
55 : // @Description: BendyRuler will avoid changing bearing unless ratio of previous margin from obstacle (or fence) to present calculated margin is atleast this much.
56 : // @Range: 1.1 2
57 : // @Increment: 0.1
58 : // @User: Standard
59 : AP_GROUPINFO("CONT_RATIO", 2, AP_OABendyRuler, _bendy_ratio, OA_BENDYRULER_RATIO_DEFAULT),
60 :
61 : // @Param: CONT_ANGLE
62 : // @DisplayName: BendyRuler's bearing change resistance threshold angle
63 : // @Description: BendyRuler will resist changing current bearing if the change in bearing is over this angle
64 : // @Range: 20 180
65 : // @Increment: 5
66 : // @User: Standard
67 : AP_GROUPINFO("CONT_ANGLE", 3, AP_OABendyRuler, _bendy_angle, OA_BENDYRULER_ANGLE_DEFAULT),
68 :
69 : // @Param{Copter}: TYPE
70 : // @DisplayName: Type of BendyRuler
71 : // @Description: BendyRuler will search for clear path along the direction defined by this parameter
72 : // @Values: 1:Horizontal search, 2:Vertical search
73 : // @User: Standard
74 : AP_GROUPINFO_FRAME("TYPE", 4, AP_OABendyRuler, _bendy_type, OA_BENDYRULER_TYPE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
75 :
76 : AP_GROUPEND
77 : };
78 :
79 2 : AP_OABendyRuler::AP_OABendyRuler()
80 : {
81 2 : AP_Param::setup_object_defaults(this, var_info);
82 2 : _bearing_prev = FLT_MAX;
83 2 : }
84 :
85 : // run background task to find best path
86 : // returns true and updates origin_new and destination_new if a best path has been found. returns false if OA is not required
87 : // bendy_type is set to the type of BendyRuler used
88 296 : bool AP_OABendyRuler::update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, OABendyType &bendy_type, bool proximity_only)
89 : {
90 : // bendy ruler always sets origin to current_loc
91 296 : origin_new = current_loc;
92 :
93 : // init bendy_type returned
94 296 : bendy_type = OABendyType::OA_BENDY_DISABLED;
95 :
96 : // calculate bearing and distance to final destination
97 296 : const float bearing_to_dest = current_loc.get_bearing_to(destination) * 0.01f;
98 296 : const float distance_to_dest = current_loc.get_distance(destination);
99 :
100 : // make sure user has set a meaningful value for _lookahead
101 296 : _lookahead.set(MAX(_lookahead,1.0f));
102 :
103 : // lookahead distance is adjusted dynamically based on avoidance results
104 296 : _current_lookahead = constrain_float(_current_lookahead, _lookahead * 0.5f, _lookahead);
105 :
106 : // calculate lookahead dist and time for step1. distance can be slightly longer than
107 : // the distance to the destination to allow room to dodge after reaching the destination
108 296 : const float lookahead_step1_dist = MIN(_current_lookahead, distance_to_dest + OA_BENDYRULER_LOOKAHEAD_PAST_DEST);
109 :
110 : // calculate lookahead dist for step2
111 296 : const float lookahead_step2_dist = _current_lookahead * OA_BENDYRULER_LOOKAHEAD_STEP2_RATIO;
112 :
113 : // get ground course
114 : float ground_course_deg;
115 296 : if (ground_speed_vec.length_squared() < OA_BENDYRULER_LOW_SPEED_SQUARED) {
116 : // with zero ground speed use vehicle's heading
117 6 : ground_course_deg = AP::ahrs().get_yaw_deg();
118 : } else {
119 290 : ground_course_deg = degrees(ground_speed_vec.angle());
120 : }
121 :
122 : bool ret;
123 296 : switch (get_type()) {
124 0 : case OABendyType::OA_BENDY_VERTICAL:
125 : #if VERTICAL_ENABLED
126 0 : ret = search_vertical_path(current_loc, destination, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest, proximity_only);
127 0 : bendy_type = OABendyType::OA_BENDY_VERTICAL;
128 0 : break;
129 : #endif
130 :
131 0 : case OABendyType::OA_BENDY_HORIZONTAL:
132 : default:
133 296 : ret = search_xy_path(current_loc, destination, ground_course_deg, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest, proximity_only);
134 296 : bendy_type = OABendyType::OA_BENDY_HORIZONTAL;
135 : }
136 :
137 296 : return ret;
138 : }
139 :
140 : // Search for path in the horizontal directions
141 296 : bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only)
142 : {
143 : // check OA_BEARING_INC definition allows checking in all directions
144 : static_assert(360 % OA_BENDYRULER_BEARING_INC_XY == 0, "check 360 is a multiple of OA_BEARING_INC");
145 :
146 : // search in OA_BENDYRULER_BEARING_INC degree increments around the vehicle alternating left
147 : // and right. For each direction check if vehicle would avoid all obstacles
148 296 : float best_bearing = bearing_to_dest;
149 296 : float best_bearing_margin = -FLT_MAX;
150 296 : bool have_best_bearing = false;
151 296 : float best_margin = -FLT_MAX;
152 296 : float best_margin_bearing = best_bearing;
153 :
154 2079 : for (uint8_t i = 0; i <= (170 / OA_BENDYRULER_BEARING_INC_XY); i++) {
155 5737 : for (uint8_t bdir = 0; bdir <= 1; bdir++) {
156 : // skip duplicate check of bearing straight towards destination
157 3954 : if ((i==0) && (bdir > 0)) {
158 197 : continue;
159 : }
160 : // bearing that we are probing
161 3757 : const float bearing_delta = i * OA_BENDYRULER_BEARING_INC_XY * (bdir == 0 ? -1.0f : 1.0f);
162 3757 : const float bearing_test = wrap_180(bearing_to_dest + bearing_delta);
163 :
164 : // ToDo: add effective groundspeed calculations using airspeed
165 : // ToDo: add prediction of vehicle's position change as part of turn to desired heading
166 :
167 : // test location is projected from current location at test bearing
168 3757 : Location test_loc = current_loc;
169 3757 : test_loc.offset_bearing(bearing_test, lookahead_step1_dist);
170 :
171 : // calculate margin from obstacles for this scenario
172 3757 : float margin = calc_avoidance_margin(current_loc, test_loc, proximity_only);
173 3757 : if (margin > best_margin) {
174 1654 : best_margin_bearing = bearing_test;
175 1654 : best_margin = margin;
176 : }
177 3757 : if (margin > _margin_max) {
178 : // this bearing avoids obstacles out to the lookahead_step1_dist
179 : // now check in there is a clear path in three directions towards the destination
180 1149 : if (!have_best_bearing) {
181 294 : best_bearing = bearing_test;
182 294 : best_bearing_margin = margin;
183 294 : have_best_bearing = true;
184 855 : } else if (fabsf(wrap_180(ground_course_deg - bearing_test)) <
185 855 : fabsf(wrap_180(ground_course_deg - best_bearing))) {
186 : // replace bearing with one that is closer to our current ground course
187 312 : best_bearing = bearing_test;
188 312 : best_bearing_margin = margin;
189 : }
190 :
191 : // perform second stage test in three directions looking for obstacles
192 1149 : const float test_bearings[] { 0.0f, 45.0f, -45.0f };
193 1149 : const float bearing_to_dest2 = test_loc.get_bearing_to(destination) * 0.01f;
194 1149 : float distance2 = constrain_float(lookahead_step2_dist, OA_BENDYRULER_LOOKAHEAD_STEP2_MIN, test_loc.get_distance(destination));
195 3893 : for (uint8_t j = 0; j < ARRAY_SIZE(test_bearings); j++) {
196 3038 : float bearing_test2 = wrap_180(bearing_to_dest2 + test_bearings[j]);
197 3038 : Location test_loc2 = test_loc;
198 3038 : test_loc2.offset_bearing(bearing_test2, distance2);
199 :
200 : // calculate minimum margin to fence and obstacles for this scenario
201 3038 : float margin2 = calc_avoidance_margin(test_loc, test_loc2, proximity_only);
202 3038 : if (margin2 > _margin_max) {
203 : // if the chosen direction is directly towards the destination avoidance can be turned off
204 : // i == 0 && j == 0 implies no deviation from bearing to destination
205 294 : const bool active = (i != 0 || j != 0);
206 294 : float final_bearing = bearing_test;
207 294 : float final_margin = margin;
208 : // check if we need ignore test_bearing and continue on previous bearing
209 294 : const bool ignore_bearing_change = resist_bearing_change(destination, current_loc, active, bearing_test, lookahead_step1_dist, margin, _destination_prev,_bearing_prev, final_bearing, final_margin, proximity_only);
210 :
211 : // all good, now project in the chosen direction by the full distance
212 294 : destination_new = current_loc;
213 294 : destination_new.offset_bearing(final_bearing, MIN(distance_to_dest, lookahead_step1_dist));
214 294 : _current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
215 294 : Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, active, bearing_to_dest, 0.0f, ignore_bearing_change, final_margin, destination, destination_new);
216 294 : return active;
217 : }
218 : }
219 : }
220 : }
221 : }
222 :
223 : float chosen_bearing;
224 : float chosen_distance;
225 2 : if (have_best_bearing) {
226 : // none of the directions tested were OK for 2-step checks. Choose the direction
227 : // that was best for the first step
228 0 : chosen_bearing = best_bearing;
229 0 : chosen_distance = MAX(lookahead_step1_dist + MIN(best_bearing_margin, 0), 0);
230 0 : _current_lookahead = MIN(_lookahead, _current_lookahead * 1.05f);
231 : } else {
232 : // none of the possible paths had a positive margin. Choose
233 : // the one with the highest margin
234 2 : chosen_bearing = best_margin_bearing;
235 2 : chosen_distance = MAX(lookahead_step1_dist + MIN(best_margin, 0), 0);
236 2 : _current_lookahead = MAX(_lookahead * 0.5f, _current_lookahead * 0.9f);
237 : }
238 :
239 : // calculate new target based on best effort
240 2 : destination_new = current_loc;
241 2 : destination_new.offset_bearing(chosen_bearing, chosen_distance);
242 :
243 : // log results
244 2 : Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, true, chosen_bearing, 0.0f, false, best_margin, destination, destination_new);
245 :
246 2 : return true;
247 : }
248 :
249 : // Search for path in the vertical directions
250 0 : bool AP_OABendyRuler::search_vertical_path(const Location ¤t_loc, const Location &destination, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only)
251 : {
252 : // check OA_BEARING_INC_VERTICAL definition allows checking in all directions
253 : static_assert(360 % OA_BENDYRULER_BEARING_INC_VERTICAL == 0, "check 360 is a multiple of OA_BEARING_INC_VERTICAL");
254 0 : float best_pitch = 0.0f;
255 0 : bool have_best_pitch = false;
256 0 : float best_margin = -FLT_MAX;
257 0 : float best_margin_pitch = best_pitch;
258 0 : const uint8_t angular_limit = 180 / OA_BENDYRULER_BEARING_INC_VERTICAL;
259 :
260 0 : for (uint8_t i = 0; i <= angular_limit; i++) {
261 0 : for (uint8_t bdir = 0; bdir <= 1; bdir++) {
262 : // skip duplicate check of bearing straight towards destination or 180 degrees behind
263 0 : if (((i==0) && (bdir > 0)) || ((i == angular_limit) && (bdir > 0))) {
264 0 : continue;
265 : }
266 :
267 : // bearing that we are probing
268 0 : const float pitch_delta = i * OA_BENDYRULER_BEARING_INC_VERTICAL * (bdir == 0 ? 1.0f : -1.0f);
269 :
270 0 : Location test_loc = current_loc;
271 0 : test_loc.offset_bearing_and_pitch(bearing_to_dest, pitch_delta, lookahead_step1_dist);
272 :
273 : // calculate margin from obstacles for this scenario
274 0 : float margin = calc_avoidance_margin(current_loc, test_loc, proximity_only);
275 :
276 0 : if (margin > best_margin) {
277 0 : best_margin_pitch = pitch_delta;
278 0 : best_margin = margin;
279 : }
280 :
281 0 : if (margin > _margin_max) {
282 : // this path avoids the obstacles with the required margin, now check for the path ahead
283 0 : if (!have_best_pitch) {
284 0 : best_pitch = pitch_delta;
285 0 : have_best_pitch = true;
286 : }
287 0 : const float test_pitch_step2[] { 0.0f, 90.0f, -90.0f, 180.0f};
288 : float bearing_to_dest2;
289 0 : if (is_equal(fabsf(pitch_delta), 90.0f)) {
290 0 : bearing_to_dest2 = bearing_to_dest;
291 : } else {
292 0 : bearing_to_dest2 = test_loc.get_bearing_to(destination) * 0.01f;
293 : }
294 0 : float distance2 = constrain_float(lookahead_step2_dist, OA_BENDYRULER_LOOKAHEAD_STEP2_MIN, test_loc.get_distance(destination));
295 :
296 0 : for (uint8_t j = 0; j < ARRAY_SIZE(test_pitch_step2); j++) {
297 0 : float bearing_test2 = wrap_180(test_pitch_step2[j]);
298 0 : Location test_loc2 = test_loc;
299 0 : test_loc2.offset_bearing_and_pitch(bearing_to_dest2, bearing_test2, distance2);
300 :
301 : // calculate minimum margin to fence and obstacles for this scenario
302 0 : float margin2 = calc_avoidance_margin(test_loc, test_loc2, proximity_only);
303 0 : if (margin2 > _margin_max) {
304 : // if the chosen direction is directly towards the destination we might turn off avoidance
305 : // i == 0 && j == 0 implies no deviation from bearing to destination
306 0 : bool active = (i != 0 || j != 0);
307 0 : if (!active) {
308 : // do a sub test for proximity obstacles to confirm if we should really turn of BendyRuler
309 0 : const float sub_test_pitch_step2[] {-90.0f, 90.0f};
310 0 : for (uint8_t k = 0; k < ARRAY_SIZE(sub_test_pitch_step2); k++) {
311 0 : Location test_loc_sub_test = test_loc;
312 0 : test_loc_sub_test.offset_bearing_and_pitch(bearing_to_dest2, sub_test_pitch_step2[k], _margin_max);
313 0 : float margin_sub_test = calc_avoidance_margin(test_loc, test_loc_sub_test, true);
314 0 : if (margin_sub_test < _margin_max) {
315 : // BendyRuler will remain active
316 0 : active = true;
317 0 : break;
318 : }
319 : }
320 : }
321 : // project in the chosen direction by the full distance
322 0 : destination_new = current_loc;
323 0 : destination_new.offset_bearing_and_pitch(bearing_to_dest, pitch_delta, distance_to_dest);
324 0 : _current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
325 :
326 0 : Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new);
327 0 : return active;
328 : }
329 : }
330 : }
331 : }
332 : }
333 :
334 : float chosen_pitch;
335 0 : if (have_best_pitch) {
336 : // none of the directions tested were OK for 2-step checks. Choose the direction
337 : // that was best for the first step
338 0 : chosen_pitch = best_pitch;
339 0 : _current_lookahead = MIN(_lookahead, _current_lookahead * 1.05f);
340 : } else {
341 : // none of the possible paths had a positive margin. Choose
342 : // the one with the highest margin
343 0 : chosen_pitch = best_margin_pitch;
344 0 : _current_lookahead = MAX(_lookahead * 0.5f, _current_lookahead * 0.9f);
345 : }
346 :
347 : // calculate new target based on best effort
348 0 : destination_new = current_loc;
349 0 : destination_new.offset_bearing_and_pitch(bearing_to_dest, chosen_pitch, distance_to_dest);
350 :
351 : // log results
352 0 : Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, true, bearing_to_dest, chosen_pitch,false, best_margin, destination, destination_new);
353 :
354 0 : return true;
355 : }
356 :
357 296 : AP_OABendyRuler::OABendyType AP_OABendyRuler::get_type() const
358 : {
359 296 : switch (_bendy_type) {
360 0 : case (uint8_t)OABendyType::OA_BENDY_VERTICAL:
361 : #if VERTICAL_ENABLED
362 0 : return OABendyType::OA_BENDY_VERTICAL;
363 : #endif
364 :
365 0 : case (uint8_t)OABendyType::OA_BENDY_HORIZONTAL:
366 : default:
367 296 : return OABendyType::OA_BENDY_HORIZONTAL;
368 : }
369 : // should never reach here
370 : return OABendyType::OA_BENDY_HORIZONTAL;
371 : }
372 :
373 : /*
374 : This function is called when BendyRuler has found a bearing which is obstacles free at atleast lookahead_step1_dist and then lookahead_step2_dist from the present location
375 : In many situations, this new bearing can be either left or right of the obstacle, and BendyRuler can have a tough time deciding between the two.
376 : It has the tendency to move the vehicle back and forth, if the margin obtained is even slightly better in the newer iteration.
377 : Therefore, this method attempts to avoid changing direction of the vehicle by more than _bendy_angle degrees,
378 : unless the new margin is atleast _bendy_ratio times better than the margin with previously calculated bearing.
379 : We return true if we have resisted the change and will follow the last calculated bearing.
380 : */
381 294 : bool AP_OABendyRuler::resist_bearing_change(const Location &destination, const Location ¤t_loc, bool active, float bearing_test, float lookahead_step1_dist, float margin, Location &prev_dest, float &prev_bearing, float &final_bearing, float &final_margin, bool proximity_only) const
382 : {
383 294 : bool resisted_change = false;
384 : // see if there was a change in destination, if so, do not resist changing bearing
385 294 : bool dest_change = false;
386 294 : if (!destination.same_latlon_as(prev_dest)) {
387 4 : dest_change = true;
388 4 : prev_dest = destination;
389 : }
390 :
391 : // check if we need to resist the change in direction of the vehicle. If we have a clear path to destination, go there any how
392 294 : if (active && !dest_change && is_positive(_bendy_ratio)) {
393 : // check the change in bearing between freshly calculated and previous stored BendyRuler bearing
394 195 : if ((fabsf(wrap_180(prev_bearing-bearing_test)) > _bendy_angle) && (!is_equal(prev_bearing,FLT_MAX))) {
395 : // check margin in last bearing's direction
396 29 : Location test_loc_previous_bearing = current_loc;
397 29 : test_loc_previous_bearing.offset_bearing(wrap_180(prev_bearing), lookahead_step1_dist);
398 29 : float previous_bearing_margin = calc_avoidance_margin(current_loc,test_loc_previous_bearing, proximity_only);
399 :
400 29 : if (margin < (_bendy_ratio * previous_bearing_margin)) {
401 : // don't change direction abruptly. If margin difference is not significant, follow the last direction
402 24 : final_bearing = prev_bearing;
403 24 : final_margin = previous_bearing_margin;
404 24 : resisted_change = true;
405 : }
406 : }
407 : } else {
408 : // reset stored bearing if BendyRuler is not active or if WP has changed for unnecessary resistance to path change
409 99 : prev_bearing = FLT_MAX;
410 : }
411 294 : if (!resisted_change) {
412 : // we are not resisting the change, hence store BendyRuler's presently calculated bearing for future iterations
413 270 : prev_bearing = bearing_test;
414 : }
415 :
416 294 : return resisted_change;
417 : }
418 :
419 : // calculate minimum distance between a segment and any obstacle
420 6824 : float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end, bool proximity_only) const
421 : {
422 6824 : float margin_min = FLT_MAX;
423 :
424 : float latest_margin;
425 :
426 6824 : if (calc_margin_from_object_database(start, end, latest_margin)) {
427 0 : margin_min = MIN(margin_min, latest_margin);
428 : }
429 :
430 6824 : if (proximity_only) {
431 : // only need margin from proximity data
432 0 : return margin_min;
433 : }
434 :
435 6824 : if (calc_margin_from_circular_fence(start, end, latest_margin)) {
436 6824 : margin_min = MIN(margin_min, latest_margin);
437 : }
438 :
439 : #if VERTICAL_ENABLED
440 : // alt fence only is only needed in vertical avoidance
441 0 : if (get_type() == OABendyType::OA_BENDY_VERTICAL) {
442 0 : if (calc_margin_from_alt_fence(start, end, latest_margin)) {
443 0 : margin_min = MIN(margin_min, latest_margin);
444 : }
445 : }
446 : #endif
447 :
448 6824 : if (calc_margin_from_inclusion_and_exclusion_polygons(start, end, latest_margin)) {
449 6824 : margin_min = MIN(margin_min, latest_margin);
450 : }
451 :
452 6824 : if (calc_margin_from_inclusion_and_exclusion_circles(start, end, latest_margin)) {
453 6824 : margin_min = MIN(margin_min, latest_margin);
454 : }
455 :
456 : // return smallest margin from any obstacle
457 6824 : return margin_min;
458 : }
459 :
460 : // calculate minimum distance between a path and the circular fence (centered on home)
461 : // on success returns true and updates margin
462 6824 : bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const
463 : {
464 : #if AP_FENCE_ENABLED
465 : // exit immediately if polygon fence is not enabled
466 6824 : const AC_Fence *fence = AC_Fence::get_singleton();
467 6824 : if (fence == nullptr) {
468 0 : return false;
469 : }
470 6824 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) == 0) {
471 0 : return false;
472 : }
473 :
474 : // calculate start and end point's distance from home
475 6824 : const Location &ahrs_home = AP::ahrs().get_home();
476 6824 : const float start_dist_sq = ahrs_home.get_distance_NE(start).length_squared();
477 6824 : const float end_dist_sq = ahrs_home.get_distance_NE(end).length_squared();
478 :
479 : // get circular fence radius + margin
480 6824 : const float fence_radius_plus_margin = fence->get_radius_m() - fence->get_margin_ne_m();
481 :
482 : // margin is fence radius minus the longer of start or end distance
483 6824 : margin = fence_radius_plus_margin - sqrtf(MAX(start_dist_sq, end_dist_sq));
484 6824 : return true;
485 : #else
486 : return false;
487 : #endif // AP_FENCE_ENABLED
488 : }
489 :
490 : // calculate minimum distance between a path and the altitude fence
491 : // on success returns true and updates margin
492 0 : bool AP_OABendyRuler::calc_margin_from_alt_fence(const Location &start, const Location &end, float &margin) const
493 : {
494 : #if AP_FENCE_ENABLED
495 : // exit immediately if polygon fence is not enabled
496 0 : const AC_Fence *fence = AC_Fence::get_singleton();
497 0 : if (fence == nullptr) {
498 0 : return false;
499 : }
500 0 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) == 0) {
501 0 : return false;
502 : }
503 :
504 : int32_t alt_above_frame_cm_start, alt_above_frame_cm_end;
505 0 : if (!start.get_alt_cm(fence->get_alt_max_frame(), alt_above_frame_cm_start)) {
506 0 : return false;
507 : }
508 0 : if (!end.get_alt_cm(fence->get_alt_max_frame(), alt_above_frame_cm_end )) {
509 0 : return false;
510 : }
511 :
512 : // safe max alt = fence alt - fence margin
513 0 : const float max_fence_alt = fence->get_safe_alt_max_m();
514 0 : const float margin_start = max_fence_alt - alt_above_frame_cm_start * 0.01f;
515 0 : const float margin_end = max_fence_alt - alt_above_frame_cm_end * 0.01f;
516 :
517 : // margin is minimum distance to fence from either start or end location
518 0 : margin = MIN(margin_start,margin_end);
519 :
520 0 : return true;
521 : #else
522 : return false;
523 : #endif // AP_FENCE_ENABLED
524 : }
525 :
526 : // calculate minimum distance between a path and all inclusion and exclusion polygons
527 : // on success returns true and updates margin
528 6824 : bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const
529 : {
530 : #if AP_FENCE_ENABLED
531 6824 : const AC_Fence *fence = AC_Fence::get_singleton();
532 6824 : if (fence == nullptr) {
533 0 : return false;
534 : }
535 :
536 : // exclusion polygons enabled along with polygon fences
537 6824 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
538 0 : return false;
539 : }
540 :
541 : // return immediately if no inclusion nor exclusion polygons
542 6824 : const uint8_t num_inclusion_polygons = fence->polyfence().get_inclusion_polygon_count();
543 6824 : const uint8_t num_exclusion_polygons = fence->polyfence().get_exclusion_polygon_count();
544 6824 : if ((num_inclusion_polygons == 0) && (num_exclusion_polygons == 0)) {
545 0 : return false;
546 : }
547 :
548 : // convert start and end to offsets from EKF origin
549 6824 : Vector2f start_NE, end_NE;
550 13648 : if (!start.get_vector_xy_from_origin_NE_cm(start_NE) ||
551 6824 : !end.get_vector_xy_from_origin_NE_cm(end_NE)) {
552 0 : return false;
553 : }
554 :
555 : // get fence margin
556 6824 : const float fence_margin = fence->get_margin_ne_m();
557 :
558 : // iterate through inclusion polygons and calculate minimum margin
559 6824 : bool margin_updated = false;
560 13648 : for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
561 : uint16_t num_points;
562 6824 : const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
563 :
564 : // if outside the fence margin is the closest distance but with negative sign
565 6824 : const float sign = Polygon_outside(start_NE, boundary, num_points) ? -1.0f : 1.0f;
566 :
567 : // calculate min distance (in meters) from line to polygon
568 6824 : float margin_new = (sign * Polygon_closest_distance_line(boundary, num_points, start_NE, end_NE) * 0.01f) - fence_margin;
569 6824 : if (!margin_updated || (margin_new < margin)) {
570 6824 : margin_updated = true;
571 6824 : margin = margin_new;
572 : }
573 : }
574 :
575 : // iterate through exclusion polygons and calculate minimum margin
576 6824 : for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
577 : uint16_t num_points;
578 0 : const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
579 :
580 : // if start is inside the polygon the margin's sign is reversed
581 0 : const float sign = Polygon_outside(start_NE, boundary, num_points) ? 1.0f : -1.0f;
582 :
583 : // calculate min distance (in meters) from line to polygon
584 0 : float margin_new = (sign * Polygon_closest_distance_line(boundary, num_points, start_NE, end_NE) * 0.01f) - fence_margin;
585 0 : if (!margin_updated || (margin_new < margin)) {
586 0 : margin_updated = true;
587 0 : margin = margin_new;
588 : }
589 : }
590 :
591 6824 : return margin_updated;
592 : #else
593 : return false;
594 : #endif // AP_FENCE_ENABLED
595 : }
596 :
597 : // calculate minimum distance between a path and all inclusion and exclusion circles
598 : // on success returns true and updates margin
599 6824 : bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin) const
600 : {
601 : #if AP_FENCE_ENABLED
602 : // exit immediately if fence is not enabled
603 6824 : const AC_Fence *fence = AC_Fence::get_singleton();
604 6824 : if (fence == nullptr) {
605 0 : return false;
606 : }
607 :
608 : // inclusion/exclusion circles enabled along with polygon fences
609 6824 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
610 0 : return false;
611 : }
612 :
613 : // return immediately if no inclusion nor exclusion circles
614 6824 : const uint8_t num_inclusion_circles = fence->polyfence().get_inclusion_circle_count();
615 6824 : const uint8_t num_exclusion_circles = fence->polyfence().get_exclusion_circle_count();
616 6824 : if ((num_inclusion_circles == 0) && (num_exclusion_circles == 0)) {
617 0 : return false;
618 : }
619 :
620 : // convert start and end to offsets from EKF origin
621 6824 : Vector2f start_NE, end_NE;
622 13648 : if (!start.get_vector_xy_from_origin_NE_cm(start_NE) ||
623 6824 : !end.get_vector_xy_from_origin_NE_cm(end_NE)) {
624 0 : return false;
625 : }
626 :
627 : // get fence margin
628 6824 : const float fence_margin = fence->get_margin_ne_m();
629 :
630 : // iterate through inclusion circles and calculate minimum margin
631 6824 : bool margin_updated = false;
632 6824 : for (uint8_t i = 0; i < num_inclusion_circles; i++) {
633 0 : Vector2f center_pos_cm;
634 : float radius;
635 0 : if (fence->polyfence().get_inclusion_circle(i, center_pos_cm, radius)) {
636 :
637 : // calculate start and ends distance from the center of the circle
638 0 : const float start_dist_sq = (start_NE - center_pos_cm).length_squared();
639 0 : const float end_dist_sq = (end_NE - center_pos_cm).length_squared();
640 :
641 : // margin is fence radius minus the longer of start or end distance
642 0 : const float margin_new = (radius + fence_margin) - (sqrtf(MAX(start_dist_sq, end_dist_sq)) * 0.01f);
643 :
644 : // update margin with lowest value so far
645 0 : if (!margin_updated || (margin_new < margin)) {
646 0 : margin_updated = true;
647 0 : margin = margin_new;
648 : }
649 : }
650 : }
651 :
652 : // iterate through exclusion circles and calculate minimum margin
653 20472 : for (uint8_t i = 0; i < num_exclusion_circles; i++) {
654 13648 : Vector2f center_pos_cm;
655 : float radius;
656 13648 : if (fence->polyfence().get_exclusion_circle(i, center_pos_cm, radius)) {
657 :
658 : // first calculate distance between circle's center and segment
659 13648 : const float dist_cm = Vector2f::closest_distance_between_line_and_point(start_NE, end_NE, center_pos_cm);
660 :
661 : // margin is distance to the center minus the radius
662 13648 : const float margin_new = (dist_cm * 0.01f) - (radius + fence_margin);
663 :
664 : // update margin with lowest value so far
665 13648 : if (!margin_updated || (margin_new < margin)) {
666 10908 : margin_updated = true;
667 10908 : margin = margin_new;
668 : }
669 : }
670 : }
671 :
672 6824 : return margin_updated;
673 : #else
674 : return false;
675 : #endif // AP_FENCE_ENABLED
676 : }
677 :
678 : // calculate minimum distance between a path and proximity sensor obstacles
679 : // on success returns true and updates margin
680 6824 : bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const
681 : {
682 : // exit immediately if db is empty
683 6824 : AP_OADatabase *oaDb = AP::oadatabase();
684 6824 : if (oaDb == nullptr || !oaDb->healthy()) {
685 0 : return false;
686 : }
687 :
688 : // convert start and end to offsets (in cm) from EKF origin
689 6824 : Vector3f start_NEU,end_NEU;
690 13648 : if (!start.get_vector_from_origin_NEU_cm(start_NEU) ||
691 6824 : !end.get_vector_from_origin_NEU_cm(end_NEU)) {
692 0 : return false;
693 : }
694 6824 : if (start_NEU == end_NEU) {
695 0 : return false;
696 : }
697 :
698 : // check each obstacle's distance from segment
699 6824 : float smallest_margin = FLT_MAX;
700 6824 : for (uint16_t i=0; i<oaDb->database_count(); i++) {
701 0 : const AP_OADatabase::OA_DbItem& item = oaDb->get_item(i);
702 0 : const Vector3f point_cm = item.pos * 100.0f;
703 : // margin is distance between line segment and obstacle minus obstacle's radius
704 0 : const float m = Vector3f::closest_distance_between_line_and_point(start_NEU, end_NEU, point_cm) * 0.01f - item.radius;
705 0 : if (m < smallest_margin) {
706 0 : smallest_margin = m;
707 : }
708 : }
709 :
710 : // return smallest margin
711 6824 : if (smallest_margin < FLT_MAX) {
712 0 : margin = smallest_margin;
713 0 : return true;
714 : }
715 :
716 6824 : return false;
717 : }
718 :
719 : #endif // AP_OAPATHPLANNER_BENDYRULER_ENABLED
|
