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
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
See discussion here:
https://github.com/ArduPilot/ardupilot/issues/7331
we were getting some uninitialised variables. While it only showed up in
AP_SbusOut, it means we can't be sure it won't happen on other objects,
so safest to remove the approach
Thanks to assistance from Lucas, Peter and Francisco
Fixes from PR review including
(a) make some local variables const
(b) rename angle_max to veh_angle_max
(c) fix distance_to_lean_pct logic so distance of zero returns maximum lean angle
RC_Channel: To nullptr from NULL.
AC_Fence: To nullptr from NULL.
AC_Avoidance: To nullptr from NULL.
AC_PrecLand: To nullptr from NULL.
DataFlash: To nullptr from NULL.
SITL: To nullptr from NULL.
GCS_MAVLink: To nullptr from NULL.
DataFlash: To nullptr from NULL.
AP_Compass: To nullptr from NULL.
Global: To nullptr from NULL.
Global: To nullptr from NULL.
We should actually modify this function to scale back the z axis in order to avoid breaching the vertical fence. Currently breaching the vertical fence is handled within the position controller which is inconsistent.
