Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AC_Avoidance_config.h"
       4             : 
       5             : #if AP_OAPATHPLANNER_BENDYRULER_ENABLED
       6             : 
       7             : #include <AP_Common/AP_Common.h>
       8             : #include <AP_Common/Location.h>
       9             : #include <AP_Math/AP_Math.h>
      10             : #include <AP_Logger/AP_Logger_config.h>
      11             : 
      12             : /*
      13             :  * BendyRuler avoidance algorithm for avoiding the polygon and circular fence and dynamic objects detected by the proximity sensor
      14             :  */
      15             : class AP_OABendyRuler {
      16             : public:
      17             :     AP_OABendyRuler();
      18             : 
      19             :     CLASS_NO_COPY(AP_OABendyRuler);  /* Do not allow copies */
      20             : 
      21             :     // send configuration info stored in front end parameters
      22         296 :     void set_config(float margin_max) { _margin_max = MAX(margin_max, 0.0f); }
      23             : 
      24             :     enum class OABendyType {
      25             :         OA_BENDY_DISABLED   = 0,
      26             :         OA_BENDY_HORIZONTAL = 1,
      27             :         OA_BENDY_VERTICAL   = 2,
      28             :     };
      29             : 
      30             :     // run background task to find best path
      31             :     // returns true and updates origin_new and destination_new if a best path has been found.  returns false if OA is not required
      32             :     // bendy_type is set to the type of BendyRuler used
      33             :     bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, OABendyType &bendy_type, bool proximity_only);
      34             : 
      35             :     static const struct AP_Param::GroupInfo var_info[];
      36             : 
      37             : private:
      38             : 
      39             :     // return type of BendyRuler in use
      40             :     OABendyType get_type() const;
      41             : 
      42             :     // search for path in XY direction
      43             :     bool search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step_1_dist, float lookahead_step_2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only);
      44             : 
      45             :     // search for path in the Vertical directions
      46             :     bool search_vertical_path(const Location &current_loc, const Location &destination, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only);
      47             : 
      48             :     // calculate minimum distance between a path and any obstacle
      49             :     float calc_avoidance_margin(const Location &start, const Location &end, bool proximity_only) const;
      50             : 
      51             :     // determine if BendyRuler should accept the new bearing or try and resist it. Returns true if bearing is not changed  
      52             :     bool resist_bearing_change(const Location &destination, const Location &current_loc, bool active, float bearing_test, float lookahead_step1_dist, float margin, Location &prev_dest, float &prev_bearing, float &final_bearing, float &final_margin, bool proximity_only) const;    
      53             : 
      54             :     // calculate minimum distance between a path and the circular fence (centered on home)
      55             :     // on success returns true and updates margin
      56             :     bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const;
      57             : 
      58             :     // calculate minimum distance between a path and the altitude fence
      59             :     // on success returns true and updates margin
      60             :     bool calc_margin_from_alt_fence(const Location &start, const Location &end, float &margin) const;
      61             : 
      62             :     // calculate minimum distance between a path and all inclusion and exclusion polygons
      63             :     // on success returns true and updates margin
      64             :     bool calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const;
      65             : 
      66             :     // calculate minimum distance between a path and all inclusion and exclusion circles
      67             :     // on success returns true and updates margin
      68             :     bool calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin) const;
      69             : 
      70             :     // calculate minimum distance between a path and proximity sensor obstacles
      71             :     // on success returns true and updates margin
      72             :     bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const;
      73             : 
      74             :     // Logging function
      75             : #if HAL_LOGGING_ENABLED
      76             :     void Write_OABendyRuler(const uint8_t type, const bool active, const float target_yaw, const float target_pitch, const bool resist_chg, const float margin, const Location &final_dest, const Location &oa_dest) const;
      77             : #else
      78             :     void Write_OABendyRuler(const uint8_t type, const bool active, const float target_yaw, const float target_pitch, const bool resist_chg, const float margin, const Location &final_dest, const Location &oa_dest) const {}
      79             : #endif
      80             : 
      81             :     // OA common parameters
      82             :     float _margin_max;              // minimum distance in meters to keep from obstacles and fences
      83             :     
      84             :     // BendyRuler parameters
      85             :     AP_Float _lookahead;            // object avoidance will look this many meters ahead of vehicle
      86             :     AP_Float _bendy_ratio;          // object avoidance will avoid major directional change if change in margin ratio is less than this param
      87             :     AP_Int16 _bendy_angle;          // object avoidance will try avoiding change in direction over this much angle
      88             :     AP_Int8  _bendy_type;           // Type of BendyRuler to run
      89             :     
      90             :     // internal variables used by background thread
      91             :     float _current_lookahead;       // distance (in meters) ahead of the vehicle we are looking for obstacles
      92             :     float _bearing_prev;            // stored bearing in degrees 
      93             :     Location _destination_prev;     // previous destination, to check if there has been a change in destination
      94             : };
      95             : 
      96             : #endif  // AP_OAPATHPLANNER_BENDYRULER_ENABLED