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_AVOIDANCE_ENABLED
19 :
20 : #include "AC_Avoid.h"
21 : #include <AP_AHRS/AP_AHRS.h> // AHRS library
22 : #include <AC_Fence/AC_Fence.h> // Failsafe fence library
23 : #include <AP_Proximity/AP_Proximity.h>
24 : #include <AP_Beacon/AP_Beacon.h>
25 : #include <AP_Logger/AP_Logger.h>
26 : #include <AP_Vehicle/AP_Vehicle_Type.h>
27 : #include <stdio.h>
28 :
29 : #if !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
30 :
31 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
32 : # define AP_AVOID_BEHAVE_DEFAULT AC_Avoid::BehaviourType::BEHAVIOR_STOP
33 : #else
34 : # define AP_AVOID_BEHAVE_DEFAULT AC_Avoid::BehaviourType::BEHAVIOR_SLIDE
35 : #endif
36 :
37 : #if APM_BUILD_COPTER_OR_HELI
38 : # define AP_AVOID_ENABLE_Z 1
39 : #endif
40 :
41 : const AP_Param::GroupInfo AC_Avoid::var_info[] = {
42 :
43 : // @Param: ENABLE
44 : // @DisplayName: Avoidance control enable/disable
45 : // @Description: Enabled/disable avoidance input sources
46 : // @Bitmask: 0:UseFence,1:UseProximitySensor,2:UseBeaconFence
47 : // @User: Standard
48 : AP_GROUPINFO_FLAGS("ENABLE", 1, AC_Avoid, _enabled, AC_AVOID_DEFAULT, AP_PARAM_FLAG_ENABLE),
49 :
50 : // 2 was ANGLE_MAX (in centi-degrees)
51 :
52 : // @Param{Copter}: DIST_MAX
53 : // @DisplayName: Avoidance distance maximum in non-GPS flight modes
54 : // @Description: Distance from object at which obstacle avoidance will begin in non-GPS modes
55 : // @Units: m
56 : // @Range: 1 30
57 : // @User: Standard
58 : AP_GROUPINFO_FRAME("DIST_MAX", 3, AC_Avoid, _dist_max_m, AC_AVOID_NONGPS_DIST_MAX_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
59 :
60 : // @Param: MARGIN
61 : // @DisplayName: Avoidance distance margin in GPS modes
62 : // @Description: Vehicle will attempt to stay at least this distance (in meters) from objects while in GPS modes
63 : // @Units: m
64 : // @Range: 1 10
65 : // @User: Standard
66 : AP_GROUPINFO("MARGIN", 4, AC_Avoid, _margin_m, 2.0f),
67 :
68 : // @Param{Copter, Rover}: BEHAVE
69 : // @DisplayName: Avoidance behaviour
70 : // @Description: Avoidance behaviour (slide or stop)
71 : // @Values: 0:Slide,1:Stop
72 : // @User: Standard
73 : AP_GROUPINFO_FRAME("BEHAVE", 5, AC_Avoid, _behavior, AP_AVOID_BEHAVE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_ROVER),
74 :
75 : // @Param: BACKUP_SPD
76 : // @DisplayName: Avoidance maximum horizontal backup speed
77 : // @Description: Maximum speed that will be used to back away from obstacles horizontally in position control modes (m/s). Set zero to disable horizontal backup.
78 : // @Units: m/s
79 : // @Range: 0 2
80 : // @User: Standard
81 : AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_max_ne_ms, 0.75f),
82 :
83 : // @Param{Copter}: ALT_MIN
84 : // @DisplayName: Avoidance minimum altitude
85 : // @Description: Minimum altitude above which proximity based avoidance will start working. This requires a valid downward facing rangefinder reading to work. Set zero to disable
86 : // @Units: m
87 : // @Range: 0 6
88 : // @User: Standard
89 : AP_GROUPINFO_FRAME("ALT_MIN", 7, AC_Avoid, _alt_min_m, 0.0f, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
90 :
91 : // @Param: ACCEL_MAX
92 : // @DisplayName: Avoidance maximum acceleration
93 : // @Description: Maximum acceleration with which obstacles will be avoided with. Set zero to disable acceleration limits
94 : // @Units: m/s/s
95 : // @Range: 0 9
96 : // @User: Standard
97 : AP_GROUPINFO("ACCEL_MAX", 8, AC_Avoid, _accel_max_mss, 3.0f),
98 :
99 : // @Param: BACKUP_DZ
100 : // @DisplayName: Avoidance deadzone between stopping and backing away from obstacle
101 : // @Description: Distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles. Increase this parameter if you see vehicle going back and forth in front of obstacle.
102 : // @Units: m
103 : // @Range: 0 2
104 : // @User: Standard
105 : AP_GROUPINFO("BACKUP_DZ", 9, AC_Avoid, _backup_deadzone_m, 0.10f),
106 :
107 : // @Param: BACKZ_SPD
108 : // @DisplayName: Avoidance maximum vertical backup speed
109 : // @Description: Maximum speed that will be used to back away from obstacles vertically in height control modes (m/s). Set zero to disable vertical backup.
110 : // @Units: m/s
111 : // @Range: 0 2
112 : // @User: Standard
113 : AP_GROUPINFO("BACKZ_SPD", 10, AC_Avoid, _backup_speed_max_u_ms, 0.75),
114 :
115 : #if AP_AVOIDANCE_ALTHOLD_ENABLED
116 : // @Param{Copter}: ANG_MAX
117 : // @DisplayName: Avoidance max lean angle in non-GPS flight modes
118 : // @Description: Max lean angle used to avoid obstacles while in non-GPS modes. Set to zero to disable lean-based avoidance
119 : // @Units: deg
120 : // @Increment: 0.1
121 : // @Range: 0 45
122 : // @User: Standard
123 : AP_GROUPINFO_FRAME("ANG_MAX", 11, AC_Avoid, _angle_max_deg, 10.0, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
124 : #endif
125 :
126 : AP_GROUPEND
127 : };
128 :
129 : /// Constructor
130 1286 : AC_Avoid::AC_Avoid()
131 : {
132 1286 : _singleton = this;
133 :
134 1286 : AP_Param::setup_object_defaults(this, var_info);
135 1286 : }
136 :
137 : // convert parameters
138 1001 : void AC_Avoid::convert_params()
139 : {
140 : #if AP_AVOIDANCE_ALTHOLD_ENABLED
141 : // PARAMETER_CONVERSION - Added: Feb-2026 for ArduPilot-4.7
142 :
143 : // exit immediately if ANG_MAX has already been configured
144 : if (_angle_max_deg.configured()) {
145 : return;
146 : }
147 :
148 : // convert ANGLE_MAX (centi-degrees) to ANG_MAX (degrees)
149 : const AP_Param::ConversionInfo conversion_info[] = {
150 : { 95, 2, AP_PARAM_INT16, "AVOID_ANG_MAX" }, // AVOID_ANGLE_MAX moved to AVOID_ANG_MAX
151 : };
152 : AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0);
153 : #endif
154 1001 : }
155 :
156 : /*
157 : * This method limits velocity and calculates backaway velocity from various supported fences
158 : * Also limits vertical velocity using adjust_velocity_z method
159 : */
160 2177275 : void AC_Avoid::adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, Vector3f &backup_vel_neu_cms, float kP_z, float accel_z_cmss, float dt)
161 : {
162 : // Only horizontal component needed for most fences, since fences are 2D
163 2177275 : Vector2f desired_velocity_ne_cms{desired_vel_neu_cms.x, desired_vel_neu_cms.y};
164 :
165 : #if AP_FENCE_ENABLED || AP_BEACON_ENABLED
166 : // limit acceleration
167 2177275 : const float accel_limited_cmss = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
168 : #endif
169 :
170 : // maximum component of desired backup velocity in each quadrant
171 2177275 : Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms;
172 :
173 : #if AP_FENCE_ENABLED
174 2177275 : if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) {
175 : // Store velocity needed to back away from fence
176 2149482 : Vector2f backup_vel_fence_ne_cms;
177 :
178 2149482 : adjust_velocity_circle_fence(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_fence_ne_cms, dt);
179 2149482 : find_max_quadrant_velocity(backup_vel_fence_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms);
180 :
181 : // backup_vel_fence_ne_cms is set to zero after each fence in case the velocity is unset from previous methods
182 2149482 : backup_vel_fence_ne_cms.zero();
183 2149482 : adjust_velocity_inclusion_and_exclusion_polygons(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_fence_ne_cms, dt);
184 2149482 : find_max_quadrant_velocity(backup_vel_fence_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms);
185 :
186 2149482 : backup_vel_fence_ne_cms.zero();
187 2149482 : adjust_velocity_inclusion_circles(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_fence_ne_cms, dt);
188 2149482 : find_max_quadrant_velocity(backup_vel_fence_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms);
189 :
190 2149482 : backup_vel_fence_ne_cms.zero();
191 2149482 : adjust_velocity_exclusion_circles(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_fence_ne_cms, dt);
192 2149482 : find_max_quadrant_velocity(backup_vel_fence_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms);
193 : }
194 : #endif // AP_FENCE_ENABLED
195 :
196 : #if AP_BEACON_ENABLED
197 2177275 : if ((_enabled & AC_AVOID_STOP_AT_BEACON_FENCE) > 0) {
198 : // Store velocity needed to back away from beacon fence
199 27793 : Vector2f backup_vel_beacon_ne_cms;
200 27793 : adjust_velocity_beacon_fence(kP, accel_limited_cmss, desired_velocity_ne_cms, backup_vel_beacon_ne_cms, dt);
201 27793 : find_max_quadrant_velocity(backup_vel_beacon_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms);
202 : }
203 : #endif // AP_BEACON_ENABLED
204 :
205 : // check for vertical fence
206 2177275 : float desired_velocity_z_cms = desired_vel_neu_cms.z;
207 2177275 : float desired_backup_vel_u_cms = 0.0f;
208 2177275 : adjust_velocity_z(kP_z, accel_z_cmss, desired_velocity_z_cms, desired_backup_vel_u_cms, dt);
209 :
210 : // Desired backup velocity is sum of maximum velocity component in each quadrant
211 2177275 : const Vector2f desired_backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
212 2177275 : backup_vel_neu_cms = Vector3f{desired_backup_vel_ne_cms.x, desired_backup_vel_ne_cms.y, desired_backup_vel_u_cms};
213 2177275 : desired_vel_neu_cms = Vector3f{desired_velocity_ne_cms.x, desired_velocity_ne_cms.y, desired_velocity_z_cms};
214 2177275 : }
215 :
216 : /*
217 : * Adjusts the desired velocity so that the vehicle can stop
218 : * before the fence/object.
219 : * kP, accel_cmss are for the horizontal axis
220 : * kP_z, accel_z_cmss are for vertical axis
221 : */
222 2765386 : void AC_Avoid::adjust_velocity(Vector3f &desired_vel_neu_cms, bool &backing_up, float kP, float accel_cmss, float kP_z, float accel_z_cmss, float dt)
223 : {
224 : // exit immediately if disabled
225 2765386 : if (_enabled == AC_AVOID_DISABLED) {
226 588111 : return;
227 : }
228 :
229 : // make a copy of input velocity, because desired_vel_neu_cms might be changed
230 2177275 : const Vector3f desired_vel_original_neu_cms = desired_vel_neu_cms;
231 :
232 : // limit acceleration
233 2177275 : const float accel_limited_cmss = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
234 :
235 : // maximum component of horizontal desired backup velocity in each quadrant
236 2177275 : Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms;
237 2177275 : float back_vel_up_cms = 0.0f;
238 2177275 : float back_vel_down_cms = 0.0f;
239 :
240 : // Avoidance in response to proximity sensor
241 2177275 : if (proximity_avoidance_enabled() && _proximity_alt_enabled) {
242 : // Store velocity needed to back away from physical obstacles
243 2006560 : Vector3f backup_vel_proximity_neu_cms;
244 2006560 : adjust_velocity_proximity(kP, accel_limited_cmss, desired_vel_neu_cms, backup_vel_proximity_neu_cms, kP_z,accel_z_cmss, dt);
245 2006560 : find_max_quadrant_velocity_3D(backup_vel_proximity_neu_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, back_vel_up_cms, back_vel_down_cms);
246 : }
247 :
248 : // Avoidance in response to various fences
249 2177275 : Vector3f backup_vel_fence_neu_cms;
250 2177275 : adjust_velocity_fence(kP, accel_cmss, desired_vel_neu_cms, backup_vel_fence_neu_cms, kP_z, accel_z_cmss, dt);
251 2177275 : find_max_quadrant_velocity_3D(backup_vel_fence_neu_cms , quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, back_vel_up_cms, back_vel_down_cms);
252 :
253 : // Desired backup velocity is sum of maximum velocity component in each quadrant
254 2177275 : const Vector2f desired_backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
255 2177275 : const float desired_backup_vel_u_cms = back_vel_down_cms + back_vel_up_cms;
256 2177275 : Vector3f desired_backup_vel_neu_cm{desired_backup_vel_ne_cms.x, desired_backup_vel_ne_cms.y, desired_backup_vel_u_cms};
257 :
258 2177275 : const float backup_speed_max_ne_cms = _backup_speed_max_ne_ms * 100.0;
259 2177275 : if (!desired_backup_vel_neu_cm.xy().is_zero() && is_positive(backup_speed_max_ne_cms)) {
260 85894 : backing_up = true;
261 : // Constrain horizontal backing away speed
262 85894 : desired_backup_vel_neu_cm.xy().limit_length(backup_speed_max_ne_cms);
263 :
264 : // let user take control if they are backing away at a greater speed than what we have calculated
265 : // this has to be done for x,y,z separately. For eg, user is doing fine in "x" direction but might need backing up in "y".
266 85894 : if (!is_zero(desired_backup_vel_neu_cm.x)) {
267 85894 : if (is_positive(desired_backup_vel_neu_cm.x)) {
268 78628 : desired_vel_neu_cms.x = MAX(desired_vel_neu_cms.x, desired_backup_vel_neu_cm.x);
269 : } else {
270 7266 : desired_vel_neu_cms.x = MIN(desired_vel_neu_cms.x, desired_backup_vel_neu_cm.x);
271 : }
272 : }
273 85894 : if (!is_zero(desired_backup_vel_neu_cm.y)) {
274 85894 : if (is_positive(desired_backup_vel_neu_cm.y)) {
275 36780 : desired_vel_neu_cms.y = MAX(desired_vel_neu_cms.y, desired_backup_vel_neu_cm.y);
276 : } else {
277 49114 : desired_vel_neu_cms.y = MIN(desired_vel_neu_cms.y, desired_backup_vel_neu_cm.y);
278 : }
279 : }
280 : }
281 :
282 2177275 : const float backup_speed_max_u_cms = _backup_speed_max_u_ms * 100.0;
283 2177275 : if (!is_zero(desired_backup_vel_neu_cm.z) && is_positive(backup_speed_max_u_cms)) {
284 0 : backing_up = true;
285 :
286 : // Constrain vertical backing away speed
287 0 : desired_backup_vel_neu_cm.z = constrain_float(desired_backup_vel_neu_cm.z, -backup_speed_max_u_cms, backup_speed_max_u_cms);
288 :
289 0 : if (!is_zero(desired_backup_vel_neu_cm.z)) {
290 0 : if (is_positive(desired_backup_vel_neu_cm.z)) {
291 0 : desired_vel_neu_cms.z = MAX(desired_vel_neu_cms.z, desired_backup_vel_neu_cm.z);
292 : } else {
293 0 : desired_vel_neu_cms.z = MIN(desired_vel_neu_cms.z, desired_backup_vel_neu_cm.z);
294 : }
295 : }
296 : }
297 :
298 : // limit acceleration
299 2177275 : limit_accel_NEU_cm(desired_vel_original_neu_cms, desired_vel_neu_cms, dt);
300 :
301 2177275 : if (desired_vel_original_neu_cms != desired_vel_neu_cms) {
302 174418 : _last_limit_time = AP_HAL::millis();
303 : }
304 :
305 : #if HAL_LOGGING_ENABLED
306 2177275 : if (limits_active()) {
307 : // log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!)
308 181963 : uint32_t now = AP_HAL::millis();
309 181963 : if ((now - _last_log_ms) > 100) {
310 4496 : _last_log_ms = now;
311 4496 : Write_SimpleAvoidance(true, desired_vel_original_neu_cms, desired_vel_neu_cms, backing_up);
312 : }
313 : } else {
314 : // avoidance isn't active anymore
315 : // log once so that it registers in logs
316 1995312 : if (_last_log_ms) {
317 25 : Write_SimpleAvoidance(false, desired_vel_original_neu_cms, desired_vel_neu_cms, backing_up);
318 : // this makes sure logging won't run again till it is active
319 25 : _last_log_ms = 0;
320 : }
321 : }
322 : #endif
323 : }
324 :
325 : /*
326 : * Limit acceleration so that change of velocity output by avoidance library is controlled
327 : * This helps reduce jerks and sudden movements in the vehicle
328 : */
329 2177275 : void AC_Avoid::limit_accel_NEU_cm(const Vector3f &original_vel_neu_cms, Vector3f &modified_vel_neu_cms, float dt)
330 : {
331 2177275 : if (original_vel_neu_cms == modified_vel_neu_cms || is_zero(_accel_max_mss) || !is_positive(dt)) {
332 : // we can't limit accel if any of these conditions are true
333 2002857 : return;
334 : }
335 :
336 174418 : if (AP_HAL::millis() - _last_limit_time > AC_AVOID_ACCEL_TIMEOUT_MS) {
337 : // reset this velocity because its been a long time since avoidance was active
338 35 : _prev_avoid_vel_neu_cms = original_vel_neu_cms;
339 : }
340 :
341 : // acceleration demanded by avoidance
342 174418 : const Vector3f accel_neu_cmss = (modified_vel_neu_cms - _prev_avoid_vel_neu_cms)/dt;
343 :
344 : // max accel in cm
345 174418 : const float accel_max_cmss = _accel_max_mss * 100.0f;
346 :
347 174418 : if (accel_neu_cmss.length() > accel_max_cmss) {
348 : // pull back on the acceleration
349 4566 : const Vector3f accel_direction_neu = accel_neu_cmss.normalized();
350 4566 : modified_vel_neu_cms = (accel_direction_neu * accel_max_cmss) * dt + _prev_avoid_vel_neu_cms;
351 : }
352 :
353 174418 : _prev_avoid_vel_neu_cms = modified_vel_neu_cms;
354 174418 : return;
355 : }
356 :
357 : // This method is used in most Rover modes and not in Copter
358 : // adjust desired horizontal speed so that the vehicle stops before the fence or object
359 : // accel (maximum acceleration/deceleration) is in m/s/s
360 : // heading is in radians
361 : // speed is in m/s
362 : // kP should be zero for linear response, non-zero for non-linear response
363 37524 : void AC_Avoid::adjust_speed(float kP, float accel_mss, float heading_rad, float &speed_ms, float dt)
364 : {
365 : // convert heading and speed into velocity vector
366 : Vector3f vel_neu_cms{
367 37524 : cosf(heading_rad) * speed_ms * 100.0f,
368 37524 : sinf(heading_rad) * speed_ms * 100.0f,
369 : 0.0f
370 37524 : };
371 :
372 37524 : bool backing_up = false;
373 37524 : adjust_velocity(vel_neu_cms, backing_up, kP, accel_mss * 100.0f, 0, 0, dt);
374 37524 : const Vector2f vel_ne_cms{vel_neu_cms.x, vel_neu_cms.y};
375 :
376 37524 : if (backing_up) {
377 : // back up
378 0 : if (fabsf(wrap_180(degrees(vel_ne_cms.angle())) - degrees(heading_rad)) > 90.0f) {
379 : // Big difference between the direction of velocity vector and actual heading therefore we need to reverse the direction
380 0 : speed_ms = -vel_ne_cms.length() * 0.01f;
381 : } else {
382 0 : speed_ms = vel_ne_cms.length() * 0.01f;
383 : }
384 0 : return;
385 : }
386 :
387 : // No need to back up so adjust speed towards zero if needed
388 37524 : if (is_negative(speed_ms)) {
389 106 : speed_ms = -vel_ne_cms.length() * 0.01f;
390 : } else {
391 37418 : speed_ms = vel_ne_cms.length() * 0.01f;
392 : }
393 : }
394 :
395 : // adjust vertical climb rate so vehicle does not break the vertical fence
396 6299913 : void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt) {
397 6299913 : float backup_speed_cms = 0.0f;
398 6299913 : adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed_cms, dt);
399 6299913 : if (!is_zero(backup_speed_cms)) {
400 73297 : if (is_negative(backup_speed_cms)) {
401 43296 : climb_rate_cms = MIN(climb_rate_cms, backup_speed_cms);
402 : } else {
403 30001 : climb_rate_cms = MAX(climb_rate_cms, backup_speed_cms);
404 : }
405 : }
406 6299913 : }
407 :
408 : // adjust vertical climb rate so vehicle does not break the vertical fence
409 8477188 : void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed_cms, float dt)
410 : {
411 : #ifdef AP_AVOID_ENABLE_Z
412 :
413 : // exit immediately if disabled
414 8274024 : if (_enabled == AC_AVOID_DISABLED) {
415 6917211 : return;
416 : }
417 :
418 : // do not adjust climb_rate if level
419 7698731 : if (is_zero(climb_rate_cms)) {
420 6268570 : return;
421 : }
422 :
423 1430161 : const AP_AHRS &_ahrs = AP::ahrs();
424 : // limit acceleration
425 1430161 : const float accel_limited_cmss = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
426 :
427 1430161 : bool limit_min_alt = false;
428 1430161 : bool limit_max_alt = false;
429 1430161 : float max_alt_diff_m = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit)
430 1430161 : float min_alt_diff_m = 0.0f;
431 : #if AP_FENCE_ENABLED
432 : // calculate distance below fence
433 1430161 : AC_Fence *fence = AP::fence();
434 1430161 : if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence) {
435 : // calculate distance from vehicle to safe altitude
436 1423880 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) {
437 : float veh_alt_m;
438 63705 : if (fence->get_alt_in_alt_min_frame_m(veh_alt_m)) {
439 : // fence.get_safe_alt_min_m() is UP, veh_alt_m is UP:
440 63705 : min_alt_diff_m = veh_alt_m - fence->get_safe_alt_min_m();
441 63705 : limit_min_alt = true;
442 : }
443 : }
444 1423880 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) {
445 : float veh_alt_m;
446 101089 : if (fence->get_alt_in_alt_max_frame_m(veh_alt_m)) {
447 : // fence.get_safe_alt_max_m() is UP, veh_alt_m is UP:
448 101089 : max_alt_diff_m = fence->get_safe_alt_max_m() - veh_alt_m;
449 101089 : limit_max_alt = true;
450 : }
451 : }
452 : }
453 : #endif
454 :
455 : // calculate distance to (e.g.) optical flow altitude limit
456 : // AHRS values are always in metres
457 : float alt_limit_m;
458 : float curr_alt_m;
459 1455305 : if (_ahrs.get_hgt_ctrl_limit(alt_limit_m) &&
460 25144 : _ahrs.get_relative_position_D_origin_float(curr_alt_m)) {
461 : // alt_limit_m is UP, curr_alt_m is DOWN:
462 25144 : const float ctrl_alt_diff_m = alt_limit_m + curr_alt_m;
463 25144 : if (!limit_max_alt || ctrl_alt_diff_m < max_alt_diff_m) {
464 25144 : max_alt_diff_m = ctrl_alt_diff_m;
465 25144 : limit_max_alt = true;
466 : }
467 : }
468 :
469 : #if HAL_PROXIMITY_ENABLED
470 : // get distance from proximity sensor
471 : float proximity_alt_diff_m;
472 1430161 : AP_Proximity *proximity = AP::proximity();
473 1430161 : if (proximity && proximity_avoidance_enabled() && proximity->get_upward_distance(proximity_alt_diff_m)) {
474 6209 : proximity_alt_diff_m -= _margin_m;
475 6209 : if (!limit_max_alt || proximity_alt_diff_m < max_alt_diff_m) {
476 6209 : max_alt_diff_m = proximity_alt_diff_m;
477 6209 : limit_max_alt = true;
478 : }
479 : }
480 : #endif
481 :
482 : // limit climb rate
483 1430161 : if (limit_max_alt || limit_min_alt) {
484 171907 : const float max_back_spd_cms = _backup_speed_max_u_ms * 100.0;
485 : // do not allow climbing if we've breached the safe altitude
486 171907 : if (max_alt_diff_m <= 0.0f && limit_max_alt) {
487 43342 : climb_rate_cms = MIN(climb_rate_cms, 0.0f);
488 : // also calculate backup speed that will get us back to safe altitude
489 43342 : if (is_positive(max_back_spd_cms)) {
490 43342 : backup_speed_cms = -1*(get_max_speed(kP, accel_limited_cmss, -max_alt_diff_m * 100.0f, dt));
491 :
492 : // Constrain to max backup speed
493 43342 : backup_speed_cms = MAX(backup_speed_cms, -max_back_spd_cms);
494 : }
495 73348 : return;
496 : // do not allow descending if we've breached the safe altitude
497 128565 : } else if (min_alt_diff_m <= 0.0f && limit_min_alt) {
498 30006 : climb_rate_cms = MAX(climb_rate_cms, 0.0f);
499 : // also calculate backup speed that will get us back to safe altitude
500 30006 : if (is_positive(max_back_spd_cms)) {
501 30006 : backup_speed_cms = get_max_speed(kP, accel_limited_cmss, -min_alt_diff_m * 100.0f, dt);
502 :
503 : // Constrain to max backup speed
504 30006 : backup_speed_cms = MIN(backup_speed_cms, max_back_spd_cms);
505 : }
506 30006 : return;
507 : }
508 :
509 : // limit climb rate
510 98559 : if (limit_max_alt) {
511 78034 : const float max_alt_max_speed_cms = get_max_speed(kP, accel_limited_cmss, max_alt_diff_m * 100.0f, dt);
512 78034 : climb_rate_cms = MIN(max_alt_max_speed_cms, climb_rate_cms);
513 : }
514 :
515 98559 : if (limit_min_alt) {
516 29111 : const float max_alt_min_speed = get_max_speed(kP, accel_limited_cmss, min_alt_diff_m * 100.0f, dt);
517 29111 : climb_rate_cms = MAX(-max_alt_min_speed, climb_rate_cms);
518 : }
519 : }
520 : #endif
521 203164 : }
522 :
523 : #if AP_AVOIDANCE_ALTHOLD_ENABLED
524 : // adjust roll-pitch to push vehicle away from objects
525 : // roll and pitch value are in radians
526 : // veh_angle_max_rad is the user defined maximum lean angle for the vehicle in radians
527 : void AC_Avoid::adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float veh_angle_max_rad) const
528 : {
529 : // exit immediately if proximity based avoidance is disabled
530 : if (!proximity_avoidance_enabled()) {
531 : return;
532 : }
533 :
534 : // exit immediately if angle max is zero
535 : if (_angle_max_deg <= 0.0f || veh_angle_max_rad <= 0.0f) {
536 : return;
537 : }
538 :
539 : float roll_positive_norm = 0.0f; // maximum positive roll value
540 : float roll_negative_norm = 0.0f; // minimum negative roll value
541 : float pitch_positive_norm = 0.0f; // maximum positive pitch value
542 : float pitch_negative_norm = 0.0f; // minimum negative pitch value
543 :
544 : // get maximum positive and negative roll and pitch percentages from proximity sensor
545 : get_proximity_roll_pitch_norm(roll_positive_norm, roll_negative_norm, pitch_positive_norm, pitch_negative_norm);
546 :
547 : // add maximum positive and negative percentages together for roll and pitch, convert to radians
548 : Vector2f rp_out_rad((roll_positive_norm + roll_negative_norm) * radians(45.0), (pitch_positive_norm + pitch_negative_norm) * radians(45.0));
549 :
550 : // apply avoidance angular limits
551 : // the object avoidance lean angle is never more than 75% of the total angle-limit to allow the pilot to override
552 : const float angle_limit_rad = constrain_float(radians(_angle_max_deg), 0.0f, veh_angle_max_rad * AC_AVOID_ANGLE_MAX_PERCENT);
553 : float vec_length_rad = rp_out_rad.length();
554 : if (vec_length_rad > angle_limit_rad) {
555 : rp_out_rad *= (angle_limit_rad / vec_length_rad);
556 : }
557 :
558 : // add passed in roll, pitch angles
559 : rp_out_rad.x += roll_rad;
560 : rp_out_rad.y += pitch_rad;
561 :
562 : // apply total angular limits
563 : vec_length_rad = rp_out_rad.length();
564 : if (vec_length_rad > veh_angle_max_rad) {
565 : rp_out_rad *= (veh_angle_max_rad / vec_length_rad);
566 : }
567 :
568 : // return adjusted roll, pitch
569 : roll_rad = rp_out_rad.x;
570 : pitch_rad = rp_out_rad.y;
571 : }
572 : #endif // AP_AVOIDANCE_ALTHOLD_ENABLED
573 :
574 : /*
575 : * Note: This method is used to limit velocity horizontally only
576 : * Limits the component of desired_vel in the direction of the unit vector
577 : * limit_direction_ne to be at most the maximum speed permitted by the limit_distance.
578 : *
579 : * The function is unit-agnostic — accel, desired_vel_ne, and limit_direction_ne
580 : * must all use the same base unit (e.g. m, cm) for correct scaling.
581 : *
582 : * Uses velocity adjustment idea from Randy's second email on this thread:
583 : * https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
584 : */
585 986104 : void AC_Avoid::limit_velocity_NE(float kP, float accel, Vector2f &desired_vel_ne, const Vector2f& limit_direction_ne, float limit_distance, float dt) const
586 : {
587 986104 : const float max_speed = get_max_speed(kP, accel, limit_distance, dt);
588 : // project onto limit direction
589 986104 : const float speed = desired_vel_ne * limit_direction_ne;
590 986104 : if (speed > max_speed) {
591 : // subtract difference between desired speed and maximum acceptable speed
592 62679 : desired_vel_ne += limit_direction_ne * (max_speed - speed);
593 : }
594 986104 : }
595 :
596 : /*
597 : * Note: This method is used to limit velocity horizontally and vertically given a 3D desired velocity vector
598 : * Limits the component of desired_vel_neu in the direction of the obstacle_vector_neu based on the passed value of "margin"
599 : *
600 : * The function is unit-agnostic — accel, desired_vel_neu, obstacle_vector_neu, margin, and accel_u
601 : * must all use the same base unit (e.g. m, cm) for correct scaling.
602 : */
603 324869 : void AC_Avoid::limit_velocity_NEU(float kP, float accel, Vector3f &desired_vel_neu, const Vector3f& obstacle_vector_neu, float margin, float kP_z, float accel_u, float dt) const
604 : {
605 324869 : if (desired_vel_neu.is_zero()) {
606 : // nothing to limit
607 324869 : return;
608 : }
609 : // create a margin length vector in the direction of desired_vel_cms
610 : // this will create larger margin towards the direction vehicle is travelling in
611 324869 : const Vector3f margin_vector_neu = desired_vel_neu.normalized() * margin;
612 324869 : const Vector2f limit_direction_ne{obstacle_vector_neu.x, obstacle_vector_neu.y};
613 :
614 324869 : if (!limit_direction_ne.is_zero()) {
615 324869 : const float distance_from_fence_xy = MAX((limit_direction_ne.length() - Vector2f{margin_vector_neu.x, margin_vector_neu.y}.length()), 0.0f);
616 324869 : Vector2f velocity_ne{desired_vel_neu.x, desired_vel_neu.y};
617 324869 : limit_velocity_NE(kP, accel, velocity_ne, limit_direction_ne.normalized(), distance_from_fence_xy, dt);
618 324869 : desired_vel_neu.x = velocity_ne.x;
619 324869 : desired_vel_neu.y = velocity_ne.y;
620 : }
621 :
622 324869 : if (is_zero(desired_vel_neu.z) || is_zero(obstacle_vector_neu.z)) {
623 : // nothing to limit vertically if desired_vel_cms.z is zero
624 : // if obstacle_vector_neu.z is zero then the obstacle is probably horizontally located, and we can move vertically
625 324869 : return;
626 : }
627 :
628 0 : if (is_positive(desired_vel_neu.z) != is_positive(obstacle_vector_neu.z)) {
629 : // why limit velocity vertically when we are going the opposite direction
630 0 : return;
631 : }
632 :
633 : // to check if Z velocity changes
634 0 : const float velocity_original_u = desired_vel_neu.z;
635 0 : const float speed_u = fabsf(desired_vel_neu.z);
636 :
637 : // obstacle_vector_neu.z and margin_vector_neu.z should be in same direction as checked above
638 0 : const float dist_u = MAX(fabsf(obstacle_vector_neu.z) - fabsf(margin_vector_neu.z), 0.0f);
639 0 : if (is_zero(dist_u)) {
640 : // eliminate any vertical velocity
641 0 : desired_vel_neu.z = 0.0f;
642 : } else {
643 0 : const float max_z_speed = get_max_speed(kP_z, accel_u, dist_u, dt);
644 0 : desired_vel_neu.z = MIN(max_z_speed, speed_u);
645 : }
646 :
647 : // make sure the direction of the Z velocity did not change
648 : // we are only limiting speed here, not changing directions
649 : // check if original z velocity is positive or negative
650 0 : if (is_negative(velocity_original_u)) {
651 0 : desired_vel_neu.z = desired_vel_neu.z * -1.0f;
652 : }
653 : }
654 :
655 : /*
656 : * Compute the back away horizontal velocity required to avoid breaching margin
657 : * INPUT: This method requires the breach in margin distance (back_distance_cm), direction towards the breach (limit_direction)
658 : * It then calculates the desired backup velocity and passes it on to "find_max_quadrant_velocity" method to distribute the velocity vectors into respective quadrants
659 : * OUTPUT: The method then outputs four velocities (quad1/2/3/4_back_vel_cms), which correspond to the maximum horizontal desired backup velocity in each quadrant
660 : */
661 90997 : void AC_Avoid::calc_backup_velocity_2D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cm, Vector2f limit_direction, float dt) const
662 : {
663 90997 : if (limit_direction.is_zero()) {
664 : // protect against divide by zero
665 0 : return;
666 : }
667 : // speed required to move away the exact distance that we have breached the margin with
668 90997 : const float back_speed_cms = get_max_speed(kP, 0.4f * accel_cmss, fabsf(back_distance_cm), dt);
669 :
670 : // direction to the obstacle
671 90997 : limit_direction.normalize();
672 :
673 : // move in the opposite direction with the required speed
674 90997 : Vector2f back_direction_vel_cms = limit_direction * (-back_speed_cms);
675 : // divide the vector into quadrants, find maximum velocity component in each quadrant
676 90997 : find_max_quadrant_velocity(back_direction_vel_cms, quad1_back_vel_cms, quad2_back_vel_cms, quad3_back_vel_cms, quad4_back_vel_cms);
677 : }
678 :
679 : /*
680 : * Compute the back away velocity required to avoid breaching margin, including vertical component
681 : * min_z_vel is <= 0, and stores the greatest velocity in the downwards direction
682 : * max_z_vel is >= 0, and stores the greatest velocity in the upwards direction
683 : * eventually max_z_vel + min_z_vel will give the final desired Z backaway velocity
684 : */
685 6764 : void AC_Avoid::calc_backup_velocity_3D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms,
686 : float back_distance_cms, Vector3f limit_direction_neu, float kp_z, float accel_z_cmss, float back_distance_u_cm, float& min_vel_u_cms, float& max_vel_u_cms, float dt) const
687 : {
688 : // backup horizontally
689 6764 : if (is_positive(back_distance_cms)) {
690 6764 : Vector2f limit_direction_ne{limit_direction_neu.x, limit_direction_neu.y};
691 6764 : calc_backup_velocity_2D(kP, accel_cmss, quad1_back_vel_cms, quad2_back_vel_cms, quad3_back_vel_cms, quad4_back_vel_cms, back_distance_cms, limit_direction_ne, dt);
692 : }
693 :
694 : // backup vertically
695 6764 : if (!is_zero(back_distance_u_cm)) {
696 0 : float back_speed_z_cms = get_max_speed(kp_z, 0.4f * accel_z_cmss, fabsf(back_distance_u_cm), dt);
697 : // Down is positive
698 0 : if (is_positive(back_distance_u_cm)) {
699 0 : back_speed_z_cms *= -1.0f;
700 : }
701 :
702 : // store the z backup speed into min or max z if possible
703 0 : if (back_speed_z_cms < min_vel_u_cms) {
704 0 : min_vel_u_cms = back_speed_z_cms;
705 : }
706 0 : if (back_speed_z_cms > max_vel_u_cms) {
707 0 : max_vel_u_cms = back_speed_z_cms;
708 : }
709 : }
710 6764 : }
711 :
712 : /*
713 : * Calculate maximum velocity vector that can be formed in each quadrant
714 : * This method takes the desired backup velocity, and four other velocities corresponding to each quadrant
715 : * The desired velocity is then fit into one of the 4 quadrant velocities as per the sign of its components
716 : * This ensures that if we have multiple backup velocities, we can get the maximum of all of those velocities in each quadrant
717 : */
718 13053956 : void AC_Avoid::find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel) const
719 : {
720 13053956 : if (desired_vel.is_zero()) {
721 12766617 : return;
722 : }
723 : // first quadrant: +ve x, +ve y direction
724 287339 : if (is_positive(desired_vel.x) && is_positive(desired_vel.y)) {
725 119958 : quad1_vel = Vector2f{MAX(quad1_vel.x, desired_vel.x), MAX(quad1_vel.y,desired_vel.y)};
726 : }
727 : // second quadrant: -ve x, +ve y direction
728 287339 : if (is_negative(desired_vel.x) && is_positive(desired_vel.y)) {
729 12134 : quad2_vel = Vector2f{MIN(quad2_vel.x, desired_vel.x), MAX(quad2_vel.y,desired_vel.y)};
730 : }
731 : // third quadrant: -ve x, -ve y direction
732 287339 : if (is_negative(desired_vel.x) && is_negative(desired_vel.y)) {
733 15250 : quad3_vel = Vector2f{MIN(quad3_vel.x, desired_vel.x), MIN(quad3_vel.y,desired_vel.y)};
734 : }
735 : // fourth quadrant: +ve x, -ve y direction
736 287339 : if (is_positive(desired_vel.x) && is_negative(desired_vel.y)) {
737 139997 : quad4_vel = Vector2f{MAX(quad4_vel.x, desired_vel.x), MIN(quad4_vel.y,desired_vel.y)};
738 : }
739 : }
740 :
741 : /*
742 : Calculate maximum velocity vector that can be formed in each quadrant and separately store max & min of vertical components
743 : */
744 4183835 : void AC_Avoid::find_max_quadrant_velocity_3D(Vector3f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel, float &max_z_vel, float &min_z_vel) const
745 : {
746 : // split into horizontal and vertical components
747 4183835 : Vector2f velocity_ne{desired_vel.x, desired_vel.y};
748 4183835 : find_max_quadrant_velocity(velocity_ne, quad1_vel, quad2_vel, quad3_vel, quad4_vel);
749 :
750 : // store maximum and minimum of z
751 4183835 : if (is_positive(desired_vel.z) && (desired_vel.z > max_z_vel)) {
752 0 : max_z_vel = desired_vel.z;
753 : }
754 4183835 : if (is_negative(desired_vel.z) && (desired_vel.z < min_z_vel)) {
755 0 : min_z_vel = desired_vel.z;
756 : }
757 4183835 : }
758 :
759 : /*
760 : * Computes the speed such that the stopping distance
761 : * of the vehicle will be exactly the input distance.
762 : */
763 1320989 : float AC_Avoid::get_max_speed(float kP, float accel, float distance, float dt) const
764 : {
765 1320989 : if (is_zero(kP)) {
766 0 : return safe_sqrt(2.0f * distance * accel);
767 : } else {
768 1320989 : return sqrt_controller(distance, kP, accel, dt);
769 : }
770 : }
771 :
772 : #if AP_FENCE_ENABLED
773 :
774 : /*
775 : * Adjusts the desired velocity for the circular fence.
776 : */
777 2149482 : void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt)
778 : {
779 2149482 : AC_Fence *fence = AP::fence();
780 2149482 : if (fence == nullptr) {
781 2050124 : return;
782 : }
783 :
784 2149482 : AC_Fence &_fence = *fence;
785 :
786 : // exit if circular fence is not enabled
787 2149482 : if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) == 0) {
788 1928350 : return;
789 : }
790 :
791 : // exit if the circular fence has already been breached
792 221132 : if ((_fence.get_breaches() & AC_FENCE_TYPE_CIRCLE) != 0) {
793 0 : return;
794 : }
795 :
796 : // get desired speed
797 221132 : const float desired_speed_cms = desired_vel_ne_cms.length();
798 221132 : if (is_zero(desired_speed_cms)) {
799 : // no avoidance necessary when desired speed is zero
800 24 : return;
801 : }
802 :
803 221108 : const AP_AHRS &_ahrs = AP::ahrs();
804 :
805 : // get position as a 2D offset from ahrs home
806 221108 : Vector2f position_ne_cm;
807 221108 : if (!_ahrs.get_relative_position_NE_home(position_ne_cm)) {
808 : // we have no idea where we are....
809 0 : return;
810 : }
811 221108 : position_ne_cm *= 100.0f; // m -> cm
812 :
813 : // get the fence radius in cm
814 221108 : const float fence_radius_cm = _fence.get_radius_m() * 100.0f;
815 : // get the margin to the fence in cm
816 221108 : const float margin_cm = _fence.get_margin_ne_m() * 100.0f;
817 :
818 221108 : if (margin_cm > fence_radius_cm) {
819 0 : return;
820 : }
821 :
822 : // get vehicle distance from home
823 221108 : const float dist_from_home_cm = position_ne_cm.length();
824 221108 : if (dist_from_home_cm > fence_radius_cm) {
825 : // outside of circular fence, no velocity adjustments
826 0 : return;
827 : }
828 221108 : const float distance_to_boundary_cm = fence_radius_cm - dist_from_home_cm;
829 :
830 : // for backing away
831 221108 : Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms;
832 :
833 : // back away if vehicle has breached margin
834 221108 : if (is_negative(distance_to_boundary_cm - margin_cm)) {
835 45167 : calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, margin_cm - distance_to_boundary_cm, position_ne_cm, dt);
836 : }
837 : // desired backup velocity is sum of maximum velocity component in each quadrant
838 221108 : backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
839 :
840 : // vehicle is inside the circular fence
841 221108 : switch (_behavior) {
842 151770 : case BEHAVIOR_SLIDE: {
843 : // implement sliding behaviour
844 151770 : const Vector2f stopping_point_ne_cm = position_ne_cm + desired_vel_ne_cms * (get_stopping_distance(kP, accel_cmss, desired_speed_cms) / desired_speed_cms);
845 151770 : const float stopping_point_dist_from_home_ne_cm = stopping_point_ne_cm.length();
846 151770 : if (stopping_point_dist_from_home_ne_cm <= fence_radius_cm - margin_cm) {
847 : // stopping before before fence so no need to adjust
848 121750 : return;
849 : }
850 : // unsafe desired velocity - will not be able to stop before reaching margin from fence
851 : // Project stopping point radially onto fence boundary
852 : // Adjusted velocity will point towards this projected point at a safe speed
853 30020 : const Vector2f target_offset_ne_cm = stopping_point_ne_cm * ((fence_radius_cm - margin_cm) / stopping_point_dist_from_home_ne_cm);
854 30020 : const Vector2f target_direction_ne_cm = target_offset_ne_cm - position_ne_cm;
855 30020 : const float distance_to_target_cm = target_direction_ne_cm.length();
856 30020 : if (is_positive(distance_to_target_cm)) {
857 30020 : const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt);
858 30020 : desired_vel_ne_cms = target_direction_ne_cm * (MIN(desired_speed_cms,max_speed_cms) / distance_to_target_cm);
859 : }
860 30020 : break;
861 : }
862 :
863 69338 : case (BEHAVIOR_STOP): {
864 : // implement stopping behaviour
865 : // calculate stopping point plus a margin so we look forward far enough to intersect with circular fence
866 69338 : const Vector2f stopping_point_plus_margin_ne_cm = position_ne_cm + desired_vel_ne_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed_cms))/desired_speed_cms);
867 69338 : const float stopping_point_plus_margin_dist_from_home_cm = stopping_point_plus_margin_ne_cm.length();
868 69338 : if (dist_from_home_cm >= fence_radius_cm - margin_cm) {
869 : // vehicle has already breached margin around fence
870 : // if stopping point is even further from home (i.e. in wrong direction) then adjust speed to zero
871 : // otherwise user is backing away from fence so do not apply limits
872 25183 : if (stopping_point_plus_margin_dist_from_home_cm >= dist_from_home_cm) {
873 21597 : desired_vel_ne_cms.zero();
874 : }
875 : } else {
876 : // shorten vector without adjusting its direction
877 44155 : Vector2f intersection_ne_cm;
878 44155 : if (Vector2f::circle_segment_intersection(position_ne_cm, stopping_point_plus_margin_ne_cm, Vector2f(0.0f,0.0f), fence_radius_cm - margin_cm, intersection_ne_cm)) {
879 5821 : const float distance_to_target_cm = (intersection_ne_cm - position_ne_cm).length();
880 5821 : const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt);
881 5821 : if (max_speed_cms < desired_speed_cms) {
882 4756 : desired_vel_ne_cms *= MAX(max_speed_cms, 0.0f) / desired_speed_cms;
883 : }
884 : }
885 : }
886 69338 : break;
887 : }
888 : }
889 : }
890 :
891 : /*
892 : * Adjusts the desired velocity for the exclusion polygons
893 : */
894 2149482 : void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt)
895 : {
896 2149482 : const AC_Fence *fence = AP::fence();
897 2149482 : if (fence == nullptr) {
898 1959764 : return;
899 : }
900 :
901 : // exit if polygon fences are not enabled
902 2149482 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
903 1959764 : return;
904 : }
905 :
906 : // for backing away
907 189718 : Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms;
908 :
909 : // iterate through inclusion polygons
910 189718 : const uint8_t num_inclusion_polygons = fence->polyfence().get_inclusion_polygon_count();
911 328714 : for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
912 : uint16_t num_points;
913 138996 : const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
914 138996 : Vector2f backup_vel_inc_ne_cms;
915 : // adjust velocity
916 138996 : adjust_velocity_polygon(kP, accel_cmss, desired_vel_ne_cms, backup_vel_inc_ne_cms, boundary, num_points, fence->get_margin_ne_m(), dt, true);
917 138996 : find_max_quadrant_velocity(backup_vel_inc_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms);
918 : }
919 :
920 : // iterate through exclusion polygons
921 189718 : const uint8_t num_exclusion_polygons = fence->polyfence().get_exclusion_polygon_count();
922 204125 : for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
923 : uint16_t num_points;
924 14407 : const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
925 14407 : Vector2f backup_vel_exc_ne_cms;
926 : // adjust velocity
927 14407 : adjust_velocity_polygon(kP, accel_cmss, desired_vel_ne_cms, backup_vel_exc_ne_cms, boundary, num_points, fence->get_margin_ne_m(), dt, false);
928 14407 : find_max_quadrant_velocity(backup_vel_exc_ne_cms, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms);
929 : }
930 : // desired backup velocity is sum of maximum velocity component in each quadrant
931 189718 : backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
932 : }
933 :
934 : /*
935 : * Adjusts the desired velocity for the inclusion circles
936 : */
937 2149482 : void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt)
938 : {
939 2149482 : const AC_Fence *fence = AP::fence();
940 2149482 : if (fence == nullptr) {
941 2098760 : return;
942 : }
943 :
944 : // return immediately if no inclusion circles
945 2149482 : const uint8_t num_circles = fence->polyfence().get_inclusion_circle_count();
946 2149482 : if (num_circles == 0) {
947 2098756 : return;
948 : }
949 :
950 : // exit if polygon fences are not enabled
951 50726 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
952 4 : return;
953 : }
954 :
955 : // get vehicle position
956 50722 : Vector2f position_ne_cm;
957 50722 : if (!AP::ahrs().get_relative_position_NE_origin_float(position_ne_cm)) {
958 : // do not limit velocity if we don't have a position estimate
959 0 : return;
960 : }
961 50722 : position_ne_cm = position_ne_cm * 100.0f; // m to cm
962 :
963 : // get the margin to the fence in cm
964 50722 : const float margin_cm = fence->get_margin_ne_m() * 100.0f;
965 :
966 : // get desired speed
967 50722 : const float desired_speed_cms = desired_vel_ne_cms.length();
968 :
969 : // get stopping distance as an offset from the vehicle
970 50722 : Vector2f stopping_offset_ne_cm;
971 50722 : if (!is_zero(desired_speed_cms)) {
972 50722 : switch (_behavior) {
973 50275 : case BEHAVIOR_SLIDE:
974 50275 : stopping_offset_ne_cm = desired_vel_ne_cms * (get_stopping_distance(kP, accel_cmss, desired_speed_cms) / desired_speed_cms);
975 50275 : break;
976 447 : case BEHAVIOR_STOP:
977 : // calculate stopping point plus a margin so we look forward far enough to intersect with circular fence
978 447 : stopping_offset_ne_cm = desired_vel_ne_cms * ((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed_cms)) / desired_speed_cms);
979 447 : break;
980 : }
981 : }
982 :
983 : // for backing away
984 50722 : Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms;
985 :
986 : // iterate through inclusion circles
987 101444 : for (uint8_t i = 0; i < num_circles; i++) {
988 50722 : Vector2f center_pos_ne_cm;
989 : float radius_m;
990 50722 : if (fence->polyfence().get_inclusion_circle(i, center_pos_ne_cm, radius_m)) {
991 : // get position relative to circle's center
992 50722 : const Vector2f position_rel_ne_cm = (position_ne_cm - center_pos_ne_cm);
993 :
994 : // if we are outside this circle do not limit velocity for this circle
995 50722 : const float dist_sq_cm = position_rel_ne_cm.length_squared();
996 50722 : const float radius_cm = (radius_m * 100.0f);
997 50722 : if (dist_sq_cm > sq(radius_cm)) {
998 23168 : continue;
999 : }
1000 :
1001 50253 : const float radius_with_margin_cm = radius_cm - margin_cm;
1002 50253 : if (is_negative(radius_with_margin_cm)) {
1003 0 : return;
1004 : }
1005 :
1006 50253 : const float margin_breach_cm = radius_with_margin_cm - safe_sqrt(dist_sq_cm);
1007 : // back away if vehicle has breached margin
1008 50253 : if (is_negative(margin_breach_cm)) {
1009 11268 : calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, margin_breach_cm, position_rel_ne_cm, dt);
1010 : }
1011 50253 : if (is_zero(desired_speed_cms)) {
1012 : // no avoidance necessary when desired speed is zero
1013 0 : continue;
1014 : }
1015 :
1016 50253 : switch (_behavior) {
1017 50253 : case BEHAVIOR_SLIDE: {
1018 : // implement sliding behaviour
1019 50253 : const Vector2f stopping_point_ne_cm = position_rel_ne_cm + stopping_offset_ne_cm;
1020 50253 : const float stopping_point_dist_cm = stopping_point_ne_cm.length();
1021 50253 : if (is_zero(stopping_point_dist_cm) || (stopping_point_dist_cm <= (radius_cm - margin_cm))) {
1022 : // stopping before before fence so no need to adjust for this circle
1023 22699 : continue;
1024 : }
1025 : // unsafe desired velocity - will not be able to stop before reaching margin from fence
1026 : // project stopping point radially onto fence boundary
1027 : // adjusted velocity will point towards this projected point at a safe speed
1028 27554 : const Vector2f target_offset_ne_cm = stopping_point_ne_cm * ((radius_cm - margin_cm) / stopping_point_dist_cm);
1029 27554 : const Vector2f target_direction_ne_cm = target_offset_ne_cm - position_rel_ne_cm;
1030 27554 : const float distance_to_target_cm = target_direction_ne_cm.length();
1031 27554 : if (is_positive(distance_to_target_cm)) {
1032 27554 : const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt);
1033 27554 : desired_vel_ne_cms = target_direction_ne_cm * (MIN(desired_speed_cms, max_speed_cms) / distance_to_target_cm);
1034 : }
1035 : }
1036 27554 : break;
1037 0 : case BEHAVIOR_STOP: {
1038 : // implement stopping behaviour
1039 0 : const Vector2f stopping_point_plus_margin_ne_cm = position_rel_ne_cm + stopping_offset_ne_cm;
1040 0 : const float dist_cm = safe_sqrt(dist_sq_cm);
1041 0 : if (dist_cm >= radius_cm - margin_cm) {
1042 : // vehicle has already breached margin around fence
1043 : // if stopping point is even further from center (i.e. in wrong direction) then adjust speed to zero
1044 : // otherwise user is backing away from fence so do not apply limits
1045 0 : if (stopping_point_plus_margin_ne_cm.length() >= dist_cm) {
1046 0 : desired_vel_ne_cms.zero();
1047 : // desired backup velocity is sum of maximum velocity component in each quadrant
1048 0 : backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
1049 0 : return;
1050 : }
1051 : } else {
1052 : // shorten vector without adjusting its direction
1053 0 : Vector2f intersection_ne_cm;
1054 0 : if (Vector2f::circle_segment_intersection(position_rel_ne_cm, stopping_point_plus_margin_ne_cm, Vector2f(0.0f,0.0f), radius_cm - margin_cm, intersection_ne_cm)) {
1055 0 : const float distance_to_target_cm = (intersection_ne_cm - position_rel_ne_cm).length();
1056 0 : const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt);
1057 0 : if (max_speed_cms < desired_speed_cms) {
1058 0 : desired_vel_ne_cms *= MAX(max_speed_cms, 0.0f) / desired_speed_cms;
1059 : }
1060 : }
1061 : }
1062 : }
1063 0 : break;
1064 : }
1065 : }
1066 : }
1067 : // desired backup velocity is sum of maximum velocity component in each quadrant
1068 50722 : backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
1069 : }
1070 :
1071 : /*
1072 : * Adjusts the desired velocity for the exclusion circles
1073 : */
1074 2149482 : void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt)
1075 : {
1076 2149482 : const AC_Fence *fence = AP::fence();
1077 2149482 : if (fence == nullptr) {
1078 2121272 : return;
1079 : }
1080 :
1081 : // return immediately if no inclusion circles
1082 2149482 : const uint8_t num_circles = fence->polyfence().get_exclusion_circle_count();
1083 2149482 : if (num_circles == 0) {
1084 2121272 : return;
1085 : }
1086 :
1087 : // exit if polygon fences are not enabled
1088 28210 : if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
1089 0 : return;
1090 : }
1091 :
1092 : // get vehicle position
1093 28210 : Vector2f position_ne_cm;
1094 28210 : if (!AP::ahrs().get_relative_position_NE_origin_float(position_ne_cm)) {
1095 : // do not limit velocity if we don't have a position estimate
1096 0 : return;
1097 : }
1098 28210 : position_ne_cm = position_ne_cm * 100.0f; // m to cm
1099 :
1100 : // get the margin to the fence in cm
1101 28210 : const float margin_cm = fence->get_margin_ne_m() * 100.0f;
1102 :
1103 : // for backing away
1104 28210 : Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms;
1105 :
1106 : // get desired speed
1107 28210 : const float desired_speed_cms = desired_vel_ne_cms.length();
1108 :
1109 : // calculate stopping distance as an offset from the vehicle (only used for BEHAVIOR_STOP)
1110 : // add a margin so we look forward far enough to intersect with circular fence
1111 28210 : Vector2f stopping_offset_ne_cm;
1112 28210 : if (!is_zero(desired_speed_cms)) {
1113 28210 : if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_STOP) {
1114 28210 : stopping_offset_ne_cm = desired_vel_ne_cms * ((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed_cms)) / desired_speed_cms);
1115 : }
1116 : }
1117 : // iterate through exclusion circles
1118 71117 : for (uint8_t i = 0; i < num_circles; i++) {
1119 42907 : Vector2f center_pos_ne_cm;
1120 : float radius_m;
1121 42907 : if (fence->polyfence().get_exclusion_circle(i, center_pos_ne_cm, radius_m)) {
1122 : // get position relative to circle's center
1123 42907 : const Vector2f position_rel_ne_cm = (position_ne_cm - center_pos_ne_cm);
1124 :
1125 : // if we are inside this circle do not limit velocity for this circle
1126 42907 : const float dist_sq_cm = position_rel_ne_cm.length_squared();
1127 42907 : const float radius_cm = (radius_m * 100.0f);
1128 42907 : if (radius_cm < margin_cm) {
1129 0 : return;
1130 : }
1131 42907 : if (dist_sq_cm < sq(radius_cm)) {
1132 0 : continue;
1133 : }
1134 :
1135 42907 : const Vector2f vector_to_center_ne_cm = center_pos_ne_cm - position_ne_cm;
1136 42907 : const float dist_to_boundary_cm = vector_to_center_ne_cm.length() - radius_cm;
1137 : // back away if vehicle has breached margin
1138 42907 : if (is_negative(dist_to_boundary_cm - margin_cm)) {
1139 0 : calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, margin_cm - dist_to_boundary_cm, vector_to_center_ne_cm, dt);
1140 : }
1141 42907 : if (is_zero(desired_speed_cms)) {
1142 : // no avoidance necessary when desired speed is zero
1143 0 : continue;
1144 : }
1145 :
1146 42907 : switch (_behavior) {
1147 0 : case BEHAVIOR_SLIDE: {
1148 : // vector from current position to circle's center
1149 0 : Vector2f limit_direction_ne_cm = vector_to_center_ne_cm;
1150 0 : if (limit_direction_ne_cm.is_zero()) {
1151 : // vehicle is exactly on circle center so do not limit velocity
1152 0 : continue;
1153 : }
1154 : // calculate distance to edge of circle
1155 0 : const float limit_distance_cm = limit_direction_ne_cm.length() - radius_cm;
1156 0 : if (!is_positive(limit_distance_cm)) {
1157 : // vehicle is within circle so do not limit velocity
1158 0 : continue;
1159 : }
1160 : // vehicle is outside the circle, adjust velocity to stay outside
1161 0 : limit_direction_ne_cm.normalize();
1162 0 : limit_velocity_NE(kP, accel_cmss, desired_vel_ne_cms, limit_direction_ne_cm, MAX(limit_distance_cm - margin_cm, 0.0f), dt);
1163 : }
1164 0 : break;
1165 42907 : case BEHAVIOR_STOP: {
1166 : // implement stopping behaviour
1167 42907 : const Vector2f stopping_point_plus_margin_ne_cm = position_rel_ne_cm + stopping_offset_ne_cm;
1168 42907 : const float dist_cm = safe_sqrt(dist_sq_cm);
1169 42907 : if (dist_cm < radius_cm + margin_cm) {
1170 : // vehicle has already breached margin around fence
1171 : // if stopping point is closer to center (i.e. in wrong direction) then adjust speed to zero
1172 : // otherwise user is backing away from fence so do not apply limits
1173 0 : if (stopping_point_plus_margin_ne_cm.length() <= dist_cm) {
1174 0 : desired_vel_ne_cms.zero();
1175 0 : backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
1176 0 : return;
1177 : }
1178 : } else {
1179 : // shorten vector without adjusting its direction
1180 42907 : Vector2f intersection_ne_cm;
1181 42907 : if (Vector2f::circle_segment_intersection(position_rel_ne_cm, stopping_point_plus_margin_ne_cm, Vector2f(0.0f,0.0f), radius_cm + margin_cm, intersection_ne_cm)) {
1182 0 : const float distance_to_target_cm = (intersection_ne_cm - position_rel_ne_cm).length();
1183 0 : const float max_speed_cms = get_max_speed(kP, accel_cmss, distance_to_target_cm, dt);
1184 0 : if (max_speed_cms < desired_speed_cms) {
1185 0 : desired_vel_ne_cms *= MAX(max_speed_cms, 0.0f) / desired_speed_cms;
1186 : }
1187 : }
1188 : }
1189 : }
1190 42907 : break;
1191 : }
1192 : }
1193 : }
1194 : // desired backup velocity is sum of maximum velocity component in each quadrant
1195 28210 : backup_vel_ne_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
1196 : }
1197 : #endif // AP_FENCE_ENABLED
1198 :
1199 : #if AP_BEACON_ENABLED
1200 : /*
1201 : * Adjusts the desired velocity for the beacon fence.
1202 : */
1203 27793 : void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_ne_cms, Vector2f &backup_vel_ne_cms, float dt)
1204 : {
1205 27793 : AP_Beacon *_beacon = AP::beacon();
1206 :
1207 : // exit if the beacon is not present
1208 27793 : if (_beacon == nullptr) {
1209 0 : return;
1210 : }
1211 :
1212 : // get boundary from beacons
1213 27793 : uint16_t num_points = 0;
1214 27793 : const Vector2f* boundary = _beacon->get_boundary_points(num_points);
1215 27793 : if ((boundary == nullptr) || (num_points == 0)) {
1216 0 : return;
1217 : }
1218 :
1219 : // adjust velocity using beacon
1220 27793 : float margin_m = 0;
1221 : #if AP_FENCE_ENABLED
1222 27793 : if (AP::fence()) {
1223 27793 : margin_m = AP::fence()->get_margin_ne_m();
1224 : }
1225 : #endif
1226 27793 : adjust_velocity_polygon(kP, accel_cmss, desired_vel_ne_cms, backup_vel_ne_cms, boundary, num_points, margin_m, dt, true);
1227 : }
1228 : #endif // AP_BEACON_ENABLED
1229 :
1230 : /*
1231 : * Adjusts the desired velocity based on output from the proximity sensor
1232 : */
1233 2006560 : void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, Vector3f &backup_vel_neu_cms, float kP_z, float accel_u_cmss, float dt)
1234 : {
1235 : #if HAL_PROXIMITY_ENABLED
1236 : // exit immediately if proximity sensor is not present
1237 2006560 : AP_Proximity *proximity = AP::proximity();
1238 2006560 : if (!proximity) {
1239 1983714 : return;
1240 : }
1241 :
1242 2006560 : AP_Proximity &_proximity = *proximity;
1243 : // get total number of obstacles
1244 2006560 : const uint8_t obstacle_num = _proximity.get_obstacle_count();
1245 2006560 : if (obstacle_num == 0) {
1246 : // no obstacles
1247 0 : return;
1248 : }
1249 :
1250 2006560 : const AP_AHRS &_ahrs = AP::ahrs();
1251 :
1252 : // for backing away
1253 2006560 : Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms;
1254 2006560 : float max_back_vel_u_cms = 0.0f;
1255 2006560 : float min_back_vel_u_cms = 0.0f;
1256 :
1257 : // rotate velocity vector from earth frame to body-frame since obstacles are in body-frame
1258 2006560 : const Vector2f desired_vel_body_ne_cms = _ahrs.earth_to_body2D(Vector2f{desired_vel_neu_cms.x, desired_vel_neu_cms.y});
1259 :
1260 : // safe_vel_ne_cms will be adjusted to stay away from Proximity Obstacles
1261 2006560 : Vector3f safe_vel_neu_cms = Vector3f{desired_vel_body_ne_cms.x, desired_vel_body_ne_cms.y, desired_vel_neu_cms.z};
1262 2006560 : const Vector3f safe_vel_orig_neu_cms = safe_vel_neu_cms;
1263 :
1264 : // calc margin in cm
1265 2006560 : const float margin_cm = MAX(_margin_m * 100.0f, 0.0f);
1266 2006560 : Vector3f stopping_point_plus_margin_neu_cm;
1267 2006560 : if (!desired_vel_neu_cms.is_zero()) {
1268 : // only used for "stop mode". Pre-calculating the stopping point here makes sure we do not need to repeat the calculations under iterations.
1269 1842483 : const float speed_cms = safe_vel_neu_cms.length();
1270 1842483 : stopping_point_plus_margin_neu_cm = safe_vel_neu_cms * ((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed_cms)) / speed_cms);
1271 : }
1272 :
1273 82268960 : for (uint8_t i = 0; i<obstacle_num; i++) {
1274 : // get obstacle from proximity library
1275 80262400 : Vector3f vector_to_obstacle_neu;
1276 80262400 : if (!_proximity.get_obstacle(i, vector_to_obstacle_neu)) {
1277 : // this one is not valid
1278 79936355 : continue;
1279 : }
1280 :
1281 326045 : const float dist_to_boundary_cm = vector_to_obstacle_neu.length();
1282 326045 : if (is_zero(dist_to_boundary_cm)) {
1283 0 : continue;
1284 : }
1285 :
1286 : // back away if vehicle has breached margin
1287 326045 : if (is_negative(dist_to_boundary_cm - margin_cm)) {
1288 9676 : const float breach_dist_cm = margin_cm - dist_to_boundary_cm;
1289 : // add a deadzone so that the vehicle doesn't backup and go forward again and again
1290 9676 : const float deadzone_cm = MAX(0.0f, _backup_deadzone_m) * 100.0f;
1291 9676 : if (breach_dist_cm > deadzone_cm) {
1292 : // this vector will help us decide how much we have to back away horizontally and vertically
1293 6764 : const Vector3f margin_vector_neu_cm = vector_to_obstacle_neu.normalized() * breach_dist_cm;
1294 6764 : const float xy_back_dist = margin_vector_neu_cm.xy().length();
1295 6764 : const float z_back_dist = margin_vector_neu_cm.z;
1296 6764 : calc_backup_velocity_3D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, xy_back_dist, vector_to_obstacle_neu, kP_z, accel_u_cmss, z_back_dist, min_back_vel_u_cms, max_back_vel_u_cms, dt);
1297 : }
1298 : }
1299 :
1300 326045 : if (desired_vel_neu_cms.is_zero()) {
1301 : // cannot limit velocity if there is nothing to limit
1302 : // backing up (if needed) has already been done
1303 0 : continue;
1304 : }
1305 :
1306 326045 : switch (_behavior) {
1307 324869 : case BEHAVIOR_SLIDE: {
1308 324869 : Vector3f limit_direction_neu{vector_to_obstacle_neu};
1309 : // distance to closest point
1310 324869 : const float limit_distance_cm = limit_direction_neu.length();
1311 324869 : if (is_zero(limit_distance_cm)) {
1312 : // We are exactly on the edge, this should ideally never be possible
1313 : // i.e. do not adjust velocity.
1314 0 : continue;
1315 : }
1316 : // Adjust velocity to not violate margin.
1317 324869 : limit_velocity_NEU(kP, accel_cmss, safe_vel_neu_cms, limit_direction_neu, margin_cm, kP_z, accel_u_cmss, dt);
1318 :
1319 324869 : break;
1320 : }
1321 :
1322 1176 : case BEHAVIOR_STOP: {
1323 : // vector from current position to obstacle
1324 1176 : Vector3f limit_direction_neu;
1325 : // find closest point with line segment
1326 : // also see if the vehicle will "roughly" intersect the boundary with the projected stopping point
1327 1176 : const bool intersect = _proximity.closest_point_from_segment_to_obstacle(i, Vector3f{}, stopping_point_plus_margin_neu_cm, limit_direction_neu);
1328 1176 : if (intersect) {
1329 : // the vehicle is intersecting the plane formed by the boundary
1330 : // distance to the closest point from the stopping point
1331 0 : float limit_distance_cm = limit_direction_neu.length();
1332 0 : if (is_zero(limit_distance_cm)) {
1333 : // We are exactly on the edge, this should ideally never be possible
1334 : // i.e. do not adjust velocity.
1335 0 : return;
1336 : }
1337 0 : if (limit_distance_cm <= margin_cm) {
1338 : // we are within the margin so stop vehicle
1339 0 : safe_vel_neu_cms.zero();
1340 : } else {
1341 : // vehicle inside the given edge, adjust velocity to not violate this edge
1342 0 : limit_velocity_NEU(kP, accel_cmss, safe_vel_neu_cms, limit_direction_neu, margin_cm, kP_z, accel_u_cmss, dt);
1343 : }
1344 :
1345 0 : break;
1346 : }
1347 : }
1348 : }
1349 : }
1350 :
1351 : // desired backup velocity is sum of maximum velocity component in each quadrant
1352 2006560 : const Vector2f desired_back_vel_cms_xy = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
1353 2006560 : const float desired_back_vel_cms_z = max_back_vel_u_cms + min_back_vel_u_cms;
1354 :
1355 2006560 : if (safe_vel_neu_cms == safe_vel_orig_neu_cms && desired_back_vel_cms_xy.is_zero() && is_zero(desired_back_vel_cms_z)) {
1356 : // proximity avoidance did nothing, no point in doing the calculations below. Return early
1357 1983714 : backup_vel_neu_cms.zero();
1358 1983714 : return;
1359 : }
1360 :
1361 : // set modified desired velocity vector and back away velocity vector
1362 : // vectors were in body-frame, rotate resulting vector back to earth-frame
1363 22846 : const Vector2f safe_vel_ne_cms = _ahrs.body_to_earth2D(Vector2f{safe_vel_neu_cms.x, safe_vel_neu_cms.y});
1364 22846 : desired_vel_neu_cms = Vector3f{safe_vel_ne_cms.x, safe_vel_ne_cms.y, safe_vel_neu_cms.z};
1365 22846 : const Vector2f backup_vel_ne_cms = _ahrs.body_to_earth2D(desired_back_vel_cms_xy);
1366 22846 : backup_vel_neu_cms = Vector3f{backup_vel_ne_cms.x, backup_vel_ne_cms.y, desired_back_vel_cms_z};
1367 : #endif // HAL_PROXIMITY_ENABLED
1368 : }
1369 :
1370 : /*
1371 : * Adjusts the desired velocity for the polygon fence.
1372 : */
1373 181196 : void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel_ne_cms, const Vector2f* boundary, uint16_t num_points, float margin, float dt, bool stay_inside)
1374 : {
1375 : // exit if there are no points
1376 181196 : if (boundary == nullptr || num_points == 0) {
1377 1041 : return;
1378 : }
1379 :
1380 181196 : const AP_AHRS &_ahrs = AP::ahrs();
1381 :
1382 : // do not adjust velocity if vehicle is outside the polygon fence
1383 181196 : Vector2f position_ne_cm;
1384 181196 : if (!_ahrs.get_relative_position_NE_origin_float(position_ne_cm)) {
1385 : // boundary is in earth frame but we have no idea
1386 : // where we are
1387 0 : return;
1388 : }
1389 181196 : position_ne_cm = position_ne_cm * 100.0f; // m to cm
1390 :
1391 :
1392 : // return if we have already breached polygon
1393 181196 : const bool inside_polygon = !Polygon_outside(position_ne_cm, boundary, num_points);
1394 181196 : if (inside_polygon != stay_inside) {
1395 1041 : return;
1396 : }
1397 :
1398 : // Safe_vel will be adjusted to remain within fence.
1399 : // We need a separate vector in case adjustment fails,
1400 : // e.g. if we are exactly on the boundary.
1401 180155 : Vector2f safe_vel_ne_cms(desired_vel_cms);
1402 180155 : Vector2f desired_back_vel_cms;
1403 :
1404 : // calc margin in cm
1405 180155 : const float margin_cm = MAX(margin * 100.0f, 0.0f);
1406 :
1407 : // for stopping
1408 180155 : const float speed = safe_vel_ne_cms.length();
1409 180155 : Vector2f stopping_point_plus_margin_ne_cm;
1410 180155 : if (!desired_vel_cms.is_zero()) {
1411 180155 : stopping_point_plus_margin_ne_cm = position_ne_cm + safe_vel_ne_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed))/speed);
1412 : }
1413 :
1414 : // for backing away
1415 180155 : Vector2f quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms;
1416 :
1417 1273044 : for (uint16_t i=0; i<num_points; i++) {
1418 1092889 : uint16_t j = i+1;
1419 1092889 : if (j >= num_points) {
1420 180155 : j = 0;
1421 : }
1422 : // end points of current edge
1423 1092889 : Vector2f start = boundary[j];
1424 1092889 : Vector2f end = boundary[i];
1425 1092889 : Vector2f vector_to_boundary_ne_cm = Vector2f::closest_point(position_ne_cm, start, end) - position_ne_cm;
1426 : // back away if vehicle has breached margin
1427 1092889 : if (is_negative(vector_to_boundary_ne_cm.length() - margin_cm)) {
1428 27798 : calc_backup_velocity_2D(kP, accel_cmss, quad_1_back_vel_ne_cms, quad_2_back_vel_ne_cms, quad_3_back_vel_ne_cms, quad_4_back_vel_ne_cms, margin_cm - vector_to_boundary_ne_cm.length(), vector_to_boundary_ne_cm, dt);
1429 : }
1430 :
1431 : // exit immediately if no desired velocity
1432 1092889 : if (desired_vel_cms.is_zero()) {
1433 0 : continue;
1434 : }
1435 :
1436 1092889 : switch (_behavior) {
1437 660938 : case (BEHAVIOR_SLIDE): {
1438 : // vector from current position to closest point on current edge
1439 660938 : Vector2f limit_direction_ne_cm = vector_to_boundary_ne_cm;
1440 : // distance to closest point
1441 660938 : const float limit_distance_cm = limit_direction_ne_cm.length();
1442 660938 : if (is_zero(limit_distance_cm)) {
1443 : // We are exactly on the edge - treat this as a fence breach.
1444 : // i.e. do not adjust velocity.
1445 0 : return;
1446 : }
1447 : // We are strictly inside the given edge.
1448 : // Adjust velocity to not violate this edge.
1449 660938 : limit_direction_ne_cm /= limit_distance_cm;
1450 660938 : limit_velocity_NE(kP, accel_cmss, safe_vel_ne_cms, limit_direction_ne_cm, MAX(limit_distance_cm - margin_cm, 0.0f), dt);
1451 660938 : break;
1452 : }
1453 :
1454 431951 : case (BEHAVIOR_STOP): {
1455 : // find intersection_ne_cm with line segment
1456 431951 : Vector2f intersection_ne_cm;
1457 431951 : if (Vector2f::segment_intersection(position_ne_cm, stopping_point_plus_margin_ne_cm, start, end, intersection_ne_cm)) {
1458 : // vector from current position to point on current edge
1459 297 : Vector2f limit_direction_ne_cm = intersection_ne_cm - position_ne_cm;
1460 297 : const float limit_distance_cm = limit_direction_ne_cm.length();
1461 297 : if (is_zero(limit_distance_cm)) {
1462 : // We are exactly on the edge - treat this as a fence breach.
1463 : // i.e. do not adjust velocity.
1464 0 : return;
1465 : }
1466 297 : if (limit_distance_cm <= margin_cm) {
1467 : // we are within the margin so stop vehicle
1468 0 : safe_vel_ne_cms.zero();
1469 : } else {
1470 : // vehicle inside the given edge, adjust velocity to not violate this edge
1471 297 : limit_direction_ne_cm /= limit_distance_cm;
1472 297 : limit_velocity_NE(kP, accel_cmss, safe_vel_ne_cms, limit_direction_ne_cm, MAX(limit_distance_cm - margin_cm, 0.0f), dt);
1473 : }
1474 : }
1475 431951 : break;
1476 : }
1477 : }
1478 : }
1479 : // desired backup velocity is sum of maximum velocity component in each quadrant
1480 180155 : desired_back_vel_cms = quad_1_back_vel_ne_cms + quad_2_back_vel_ne_cms + quad_3_back_vel_ne_cms + quad_4_back_vel_ne_cms;
1481 :
1482 : // set modified desired velocity vector or back away velocity vector
1483 180155 : desired_vel_cms = safe_vel_ne_cms;
1484 180155 : backup_vel_ne_cms = desired_back_vel_cms;
1485 : }
1486 :
1487 : /*
1488 : * Computes distance required to stop, given current speed.
1489 : *
1490 : * Implementation copied from AC_PosControl.
1491 : */
1492 2322678 : float AC_Avoid::get_stopping_distance(float kP, float accel_cmss, float speed_cms) const
1493 : {
1494 : // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
1495 2322678 : if (accel_cmss <= 0.0f || is_zero(speed_cms)) {
1496 0 : return 0.0f;
1497 : }
1498 :
1499 : // handle linear deceleration
1500 2322678 : if (kP <= 0.0f) {
1501 20253 : return 0.5f * sq(speed_cms) / accel_cmss;
1502 : }
1503 :
1504 : // calculate distance within which we can stop
1505 : // accel_cmss/kP is the point at which velocity switches from linear to sqrt
1506 2302425 : if (speed_cms < accel_cmss/kP) {
1507 1891654 : return speed_cms/kP;
1508 : } else {
1509 : // accel_cmss/(2.0f*kP*kP) is the distance at which we switch from linear to sqrt response
1510 410771 : return accel_cmss/(2.0f*kP*kP) + (speed_cms*speed_cms)/(2.0f*accel_cmss);
1511 : }
1512 : }
1513 :
1514 : #if AP_AVOIDANCE_ALTHOLD_ENABLED
1515 : // convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes
1516 : float AC_Avoid::distance_m_to_lean_norm(float dist_m) const
1517 : {
1518 : // ignore objects beyond DIST_MAX
1519 : if (dist_m < 0.0f || dist_m >= _dist_max_m || _dist_max_m <= 0.0f) {
1520 : return 0.0f;
1521 : }
1522 : // inverted but linear response
1523 : return 1.0f - (dist_m / _dist_max_m);
1524 : }
1525 :
1526 : // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor
1527 : void AC_Avoid::get_proximity_roll_pitch_norm(float &roll_positive_norm, float &roll_negative_norm, float &pitch_positive_norm, float &pitch_negative_norm) const
1528 : {
1529 : #if HAL_PROXIMITY_ENABLED
1530 : AP_Proximity *proximity = AP::proximity();
1531 : if (proximity == nullptr) {
1532 : return;
1533 : }
1534 : AP_Proximity &_proximity = *proximity;
1535 : const uint8_t obj_count = _proximity.get_object_count();
1536 : // if no objects return
1537 : if (obj_count == 0) {
1538 : return;
1539 : }
1540 :
1541 : // calculate maximum roll, pitch values from objects
1542 : for (uint8_t i=0; i<obj_count; i++) {
1543 : float ang_deg, dist_m;
1544 : if (_proximity.get_object_angle_and_distance(i, ang_deg, dist_m)) {
1545 : if (dist_m < _dist_max_m) {
1546 : // convert distance to lean angle (in 0 to 1 range)
1547 : const float lean_norm = distance_m_to_lean_norm(dist_m);
1548 : // convert angle to roll and pitch lean percentages
1549 : const float angle_rad = radians(ang_deg);
1550 : const float roll_norm = -sinf(angle_rad) * lean_norm;
1551 : const float pitch_norm = cosf(angle_rad) * lean_norm;
1552 : // update roll, pitch maximums
1553 : if (roll_norm > 0.0f) {
1554 : roll_positive_norm = MAX(roll_positive_norm, roll_norm);
1555 : } else if (roll_norm < 0.0f) {
1556 : roll_negative_norm = MIN(roll_negative_norm, roll_norm);
1557 : }
1558 : if (pitch_norm > 0.0f) {
1559 : pitch_positive_norm = MAX(pitch_positive_norm, pitch_norm);
1560 : } else if (pitch_norm < 0.0f) {
1561 : pitch_negative_norm = MIN(pitch_negative_norm, pitch_norm);
1562 : }
1563 : }
1564 : }
1565 : }
1566 : #endif // HAL_PROXIMITY_ENABLED
1567 : }
1568 : #endif // AP_AVOIDANCE_ALTHOLD_ENABLED
1569 :
1570 : // singleton instance
1571 : AC_Avoid *AC_Avoid::_singleton;
1572 :
1573 : namespace AP {
1574 :
1575 8781550 : AC_Avoid *ac_avoid()
1576 : {
1577 8781550 : return AC_Avoid::get_singleton();
1578 : }
1579 :
1580 : }
1581 :
1582 : #endif // !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1583 :
1584 : #endif // AP_AVOIDANCE_ENABLED
|
