mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
af80070ed9
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
53 lines
2.3 KiB
C++
53 lines
2.3 KiB
C++
#pragma once
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Common/Location.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Proximity/AP_Proximity.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
/*
|
|
* BendyRuler avoidance algorithm for avoiding the polygon and circular fence and dynamic objects detected by the proximity sensor
|
|
*/
|
|
class AP_OABendyRuler {
|
|
public:
|
|
|
|
AP_OABendyRuler() {}
|
|
|
|
/* Do not allow copies */
|
|
AP_OABendyRuler(const AP_OABendyRuler &other) = delete;
|
|
AP_OABendyRuler &operator=(const AP_OABendyRuler&) = delete;
|
|
|
|
// send configuration info stored in front end parameters
|
|
void set_config(float lookahead, float margin_max) { _lookahead = MAX(lookahead, 1.0f); _margin_max = MAX(margin_max, 0.0f); }
|
|
|
|
// run background task to find best path and update avoidance_results
|
|
bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new);
|
|
|
|
private:
|
|
|
|
// calculate minimum distance between a path and any obstacle
|
|
float calc_avoidance_margin(const Location &start, const Location &end);
|
|
|
|
// calculate minimum distance between a path and the circular fence (centered on home)
|
|
// on success returns true and updates margin
|
|
bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin);
|
|
|
|
// calculate minimum distance between a path and the polygon fence
|
|
// on success returns true and updates margin
|
|
bool calc_margin_from_polygon_fence(const Location &start, const Location &end, float &margin);
|
|
|
|
// calculate minimum distance between a path and proximity sensor obstacles
|
|
// on success returns true and updates margin
|
|
bool calc_margin_from_proximity_sensors(const Location &start, const Location &end, float &margin);
|
|
|
|
// configuration parameters
|
|
float _lookahead; // object avoidance will look this many meters ahead of vehicle
|
|
float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle
|
|
|
|
// internal variables used by background thread
|
|
float _current_lookahead; // distance (in meters) ahead of the vehicle we are looking for obstacles
|
|
AP_Proximity::Proximity_Location _prx_locs[PROXIMITY_MAX_DIRECTION]; // buffer of locations from proximity library
|
|
uint16_t _prx_loc_count;
|
|
};
|