Commit Graph

92 Commits

Author SHA1 Message Date
Randy Mackay 9750f99b91 AP_OADatabase: send_adsb uses vehicle's current altitude 2020-05-11 10:22:27 +09:00
Randy Mackay bfa6886e50 AP_OADatabase: available regardless of hal-minimize-features
In Copter with minimize feature set the entire AP_PathPlanning feature including the database are unavailable
Rover has enough space for the database even with minimise features set
2020-05-11 10:22:23 +09:00
Randy Mackay 933db99778 AP_OAPathPlanner: slow updates to 1hz, timeout to 3sec 2020-05-11 10:22:20 +09:00
Randy Mackay a895d2e7c9 AP_OABendyRuler: integrate oadb ekf-offset change 2020-05-11 10:22:18 +09:00
Randy Mackay f474cfdf12 AP_OADatabase: replace Location with offset from origin 2020-05-11 10:22:05 +09:00
Randy Mackay c9c28abac1 AP_OABendyRuler: integrate static object radius 2020-05-11 09:50:58 +09:00
Randy Mackay 52c1f9f798 AP_OADatabase: calculate object radius based on distance and beam width
also all object database items are normal importance
2020-05-11 09:50:55 +09:00
Randy Mackay b59908e4d3 AP_OADatabase: remove unnecessary const on arguments
also swap order of angle and distance arguments to be consistent with proximity library
2020-05-11 09:50:47 +09:00
Peter Barker 83b6fdbb04 AC_Avoid: adjust for proximity status namespace change 2019-12-16 14:59:53 +09:00
Randy Mackay 0d032ecd86 AC_Avoidance: add enabled and margin accessors 2019-09-28 11:13:14 +09:00
Randy Mackay 9aeac012d2 AC_Avoid: add support for complex fence types
AP_OABendyRuler: support exclusion polygons

AP_OADijkstra: support exclusion polygons

AC_Avoid: adjust_velocity supports exclusion polygons

AC_Avoidance: handle fence::get_boundary_points returning nullptr instead of setting num_points to zero

AC_Avoidance: Dijkstra's works with only exclusion polygons

AC_Avoidance: Dijkstra: check for fence counts instead of polyfence validity

We really only care whether fences can be returned - and they won't be
returned unless they are valid

AC_Avoidance: BendyRuler: just try to get inclusion fence rather than checking validity

AC_Avoidance: BendyRuler supports exclusion circles

AC_Avoid: Dijkstra support for exclusion circles

AC_Avoid: BendyRuler support for inclusion circles

AC_Avoid: stop an inclusion/exclusion circular fences

AC_Avoid: stop at inclusion/exclusion circular fences

AC_Avoid: fixes to Dijkstra's use of inclusion/exclusion circles and polygons

AP_Avoidance: take semaphores when interacting with AHRS and polyfence

AC_Avoid: Dijkstra's fix for some_fences_enabled inclusion circles
2019-09-28 07:58:48 +09:00
Peter Barker f3b0148719 AP_OADijkstra: cope with polyfence holding boundary points 2019-09-28 07:58:48 +09:00
Peter Barker dfca32a2af AP_OABendyRuler: cope with polyfence holding boundary points 2019-09-28 07:58:48 +09:00
Peter Barker 3a7f1b882e AC_Avoidance: cope with polyfence holding boundary points 2019-09-28 07:58:48 +09:00
Randy Mackay aba4a1fdc8 AC_Avoidance: remove OADatabase check that can never be true 2019-09-10 09:15:56 +10:00
Randy Mackay a8fd04ddf9 AC_Avoidance: Dijkstra's returns oa-not-required if path has been completed 2019-08-17 09:42:43 +09:00
Randy Mackay 452be5fd94 AC_Avoidance: Dijkstra skips calcs if current loc is same as destination 2019-08-17 09:42:43 +09:00
Randy Mackay bed21b43e1 AP_OAPathPlanner: minor fix to return original origin and dest upon failure
this has no functional effect because the consumers are not using the origin_new and destination_new unless the ret_state was OA_SUCCESS
2019-08-17 09:42:43 +09:00
Randy Mackay a1a2733926 AC_Avoidance: reduce OA path planner look ahead and margin param defaults 2019-08-09 15:51:26 +09:00
Randy Mackay 488d1ab60a AC_Avoidance: replace Write_OA with Write_OABendyRuler and Write_OADijkstra 2019-08-06 08:15:45 +09:00
Tom Pittenger 9ad6d14c16 AC_Avoidance: add Object Avoidance Database 2019-08-02 09:50:13 -06:00
Randy Mackay 84ba6e859c AP_OADijkstra: redo visgraphs if polyfence is changed 2019-07-15 13:45:10 +09:00
Peter Barker ec0a93ff9c AC_Avoidance: add missing includes for AP_AHRS 2019-07-09 22:32:57 +10:00
Randy Mackay bd3d352554 AC_Avoidance: remove comment from Dijkstra's 2019-07-07 04:08:34 -07:00
Randy Mackay 655ddd4ab3 AC_Avoidance: Dijkstra's ignores invalid polygon fences 2019-07-07 04:08:34 -07:00
Randy Mackay 951ded1f51 AP_OAPathPlanner: report errors to caller 2019-07-07 06:46:26 +09:00
Randy Mackay 0b62641a46 AC_Avoidance: fix Dijkstra's iteration through polygon fence points 2019-06-25 00:32:18 +08:00
Randy Mackay 578023483c AC_Avoidance: Dijkstra's expanding arrays use 32 elements per chunk 2019-06-20 15:09:23 +09:00
Randy Mackay da645c32ff AC_Avoidance: Dijkstra's specifies expanding array chunk sizes
also add check that number of polygon points is less than 255
2019-06-20 15:09:23 +09:00
Randy Mackay 48a7f468bf AC_Avoidance: AP_OADijkstra and OAVisGraph use AP_ExpandingArray 2019-06-20 15:09:23 +09:00
Randy Mackay bba81b2794 AC_Avoidance: Dijkstra's exits early if polygon fence disabled 2019-06-14 08:06:17 +09:00
Randy Mackay cceab9c88f AC_Avoidance: fix BendyRuler when outside polygon fence 2019-06-12 14:54:33 +09:00
Randy Mackay 0a86bbdc9a AC_Avoidance: add license to cpp file 2019-06-11 13:13:22 +09:00
Randy Mackay af80070ed9 AP_OAPathPlanner: path planning around obstacles
implements both bendy ruler and dijkstra's algorithms

includes many changes after peer review including:
removed unused airspeed and turn-rate arguments
renamed constants to start with OA_BENDYRULER_
removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled
AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit
vehicle's heading is used instead of ground-course when speed is below 0.2m/s
static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly
replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX
use calc_margin_from_xxx function's return values
calc-margin-from-proximity-sensors fails if copy-locations fails
remove debug
remove unused singletons

and other changes including:
integrate fence rename to get_boundary_points
adapt to fence return point and duplicate final point from not being returned
create bendyrule and dijkstra objects on init
add pre-arm-check to ensure they've been created successfully
Dijkstra's detects polygon fence changes
minor fix to Dijkstra's return path
allow update_visgraph to accept 0,0 as extra point

and these fixes from peer reviews:
remove unused methods
add note about dangers of operator[]
reduce memory consuption of ShortPathNode
remove unused methods including print_all_nodes and print_shortest_path
replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX
update method gets check that ekf origin has been set
fix indexing bug in inner fence creation
fix whitespace error
find-node-from-id uses switch statement instead of 3 IF statements
update_visgraph comments added to clarify usage of extra_position argument
find-closest-node-idx always uses node_index instead of int16_t
static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255
some looping style changes
AP_OABendyRuler: fix to ground course calculation
AP_OAPathPlanner: pre_arm_check writes to caller's buffer
remove unnecessary hal and stdio.h includes
rename fence_ptr local variable to fence
add sanity check to OADijkstra's set_fence_margin
add sanity check to OABendyRuler's set_config
BendyRuler's test_bearing local constants changed to be float constants
BendyRuler's ahrs_home made local constant
BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead)
BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt
AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination
AP_OADijkstra: fixes after peer review
remove unnecesary initialisations
replace safe_sqrt(sq()) with length()
remove unnecessary block
comment fixes
AP_OAPathPlanner: add logging of final an intermediate location
2019-06-11 13:13:22 +09:00
Randy Mackay 607f996232 AC_Avoid: restructure logic of adjust_velocity_circle_fence 2019-06-08 09:35:36 +09:00
Randy Mackay e846840a52 AC_Avoid: fix stopping at circular fence 2019-06-08 09:35:36 +09:00
Peter Barker 35d4555be0 AC_Avoid: improve documentation of AVOID_ENABLE options 2019-06-07 17:37:00 +09:00
Peter Barker e9160cd9d5 AC_Avoid: correct iteration through edges 2019-06-04 09:06:07 +09:00
Peter Barker cd6e5b48a1 AC_Avoid: move check for zero-desired-velocity into adjust_velocity_polygon
This is the method which divides by the length of this vector; the
check belongs in here.
2019-06-04 09:06:07 +09:00
Peter Barker de9fa4c730 AC_Avoid: call Polygon_outside directly; avoids losing first point 2019-05-29 15:34:02 +10:00
Peter Barker 4be3a48275 AC_Avoid: adjust for renamed get_polygon_points 2019-05-29 15:34:02 +10:00
Peter Barker 43152a416c AC_Avoid: stop taking references to ahrs, fence, proximity and beacon 2019-05-22 07:48:02 -06:00
Peter Barker 413ef5a3b4 AC_Avoid: add singleton 2018-10-30 12:10:42 +11:00
Randy Mackay 7aae970ae7 AC_Avoid: constify a few local variables 2018-10-26 14:38:04 +09:00
Randy Mackay b7f3140441 AC_Avoid: stop includes margin for intersection calc
the simpler stop feature (as opposed to slide) was not including the margin when looking for intersections with the boundary meaning it could slow too late
2018-10-26 14:38:04 +09:00
JIMWAR 97b5af60f8 AC_Avoid: fix adjust_velocity_polygon
adjust_velocity_polygon function ignoring boundary[0]
2018-10-24 15:37:14 +09:00
murata 3577def8fd AC_Avoid: use elseif because value does not change 2018-04-23 19:45:50 +09:00
murata 29b1a6ed41 AC_Avoid: fix comment 2018-04-23 19:45:37 +09:00
Dr.-Ing. Amilcar Do Carmo Lucas 8ae4047a00 AC_Avoidance: NFC small renames and comment improvements 2018-03-06 09:30:56 +09:00
Randy Mackay be804aa74e AC_Avoid: limit velocity and get-max-speed become public
this allows AP_Follow to use these function
2018-03-05 15:08:08 +09:00