Commit Graph

101 Commits

Author SHA1 Message Date
Rishabh 7dd7565d14 AC_Avoid: Improve BendyRuler hesitancy by avoiding significant bearing change 2020-06-24 11:42:20 +09:00
Rishabh 47e4e9f7c9 AC_Avoid: Store 3D data in OA_DB and use 3D distance in BendyRuler 2020-06-19 09:45:34 +09:00
Rishabh 15f25aa2a5 AC_Avoid: Skip unnecessary retries to build inner fences 2020-06-17 16:44:57 +09:00
Rishabh 4ec8602de2 AC_Avoid: Remove unnecessary sanity checks 2020-06-17 16:43:27 +09:00
Rishabh 34b17d0993 AC_Avoid: Check if origin is set before any obstacle avoidance algorithm is run 2020-06-17 08:26:50 +09:00
Rishabh 44f0aef5e9 AP_OAPathPlanner: Add param to reset WP origin while recovering from Bendy 2020-05-30 08:55:45 +09:00
Peter Barker 002d47447e AC_Avoidance: rename APMrover2 to Rover 2020-04-14 09:50:34 +09:00
Peter Hall 556d90ca0c AC_Avoidance: keep track of velocity liminting status 2020-02-04 10:24:33 +09:00
Andrew Tridgell 48486d259f AC_Avoidance: make all semaphores recursive
the cost is very similar and this prevents an easy coding error which
can occur on less used code paths
2020-01-19 20:19:30 +11:00
Randy Mackay 2e02ee1df8 AP_OADatabase: send_adsb uses vehicle's current altitude 2020-01-01 13:38:41 +09:00
Randy Mackay 985a15709a 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-01-01 13:38:41 +09:00
Randy Mackay 50b4dd2599 AP_OAPathPlanner: slow updates to 1hz, timeout to 3sec 2020-01-01 13:38:41 +09:00
Randy Mackay 28e707466b AP_OABendyRuler: integrate oadb ekf-offset change 2020-01-01 13:38:41 +09:00
Randy Mackay fe5ba3ccdb AP_OADatabase: replace Location with offset from origin 2020-01-01 13:38:41 +09:00
Randy Mackay 37f2fc3ffe AP_OABendyRuler: integrate static object radius 2020-01-01 13:38:41 +09:00
Randy Mackay 674d631fb1 AP_OADatabase: calculate object radius based on distance and beam width
also all object database items are normal importance
2020-01-01 13:38:41 +09:00
Randy Mackay 1ba5f4626d AP_OADatabase: remove unnecessary const on arguments
also swap order of angle and distance arguments to be consistent with proximity library
2020-01-01 13:38:41 +09:00
Peter Barker 0d624785f3 AC_Avoid: adjust for proximity status namespace change 2019-10-31 11:58:15 +08: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