AC_Avoid: Extend BendyRuler to search for paths up and down

libraries/AC_Avoidance/AP_OABendyRuler.cpp

@@ -19,16 +19,21 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Logger/AP_Logger.h>
 
+// parameter defaults
 const float OA_BENDYRULER_LOOKAHEAD_DEFAULT = 15.0f;
 const float OA_BENDYRULER_RATIO_DEFAULT = 1.5f;
 const int16_t OA_BENDYRULER_ANGLE_DEFAULT = 75;
+const int16_t OA_BENDYRULER_TYPE_DEFAULT = 1;
 
-const int16_t OA_BENDYRULER_BEARING_INC = 5;            // check every 5 degrees around vehicle
+const int16_t OA_BENDYRULER_BEARING_INC_XY = 5;            // check every 5 degrees around vehicle
+const int16_t OA_BENDYRULER_BEARING_INC_VERTICAL = 90;
 const float OA_BENDYRULER_LOOKAHEAD_STEP2_RATIO = 1.0f; // step2's lookahead length as a ratio of step1's lookahead length
 const float OA_BENDYRULER_LOOKAHEAD_STEP2_MIN = 2.0f;   // step2 checks at least this many meters past step1's location
 const float OA_BENDYRULER_LOOKAHEAD_PAST_DEST = 2.0f;   // lookahead length will be at least this many meters past the destination
 const float OA_BENDYRULER_LOW_SPEED_SQUARED = (0.2f * 0.2f);    // when ground course is below this speed squared, vehicle's heading will be used
 
+#define VERTICAL_ENABLED APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+
 const AP_Param::GroupInfo AP_OABendyRuler::var_info[] = {
 
     // @Param: LOOKAHEAD
@@ -56,6 +61,13 @@ const AP_Param::GroupInfo AP_OABendyRuler::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("CONT_ANGLE", 3, AP_OABendyRuler, _bendy_angle, OA_BENDYRULER_ANGLE_DEFAULT),
 
+    // @Param{Copter}: TYPE
+    // @DisplayName: Type of BendyRuler
+    // @Description: BendyRuler will search for clear path along the direction defined by this parameter
+    // @Values: 1:Horizontal search, 2:Vertical search
+    // @User: Standard
+    AP_GROUPINFO_FRAME("TYPE", 4, AP_OABendyRuler, _bendy_type, OA_BENDYRULER_TYPE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
+
     AP_GROUPEND
 };
 
@@ -98,8 +110,27 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
         ground_course_deg = degrees(ground_speed_vec.angle());
     }
     
+    bool ret;
+    switch (get_type()) {
+        case OABendyType::OA_BENDY_VERTICAL:
+        #if VERTICAL_ENABLED 
+            ret = search_vertical_path(current_loc, destination, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest);
+            break;
+        #endif
+
+        case OABendyType::OA_BENDY_HORIZONTAL:
+        default:
+            ret = search_xy_path(current_loc, destination, ground_course_deg, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest);
+    }
+   
+    return ret;
+}
+
+// Search for path in the horizontal directions
+bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest ) 
+{
     // check OA_BEARING_INC definition allows checking in all directions
-    static_assert(360 % OA_BENDYRULER_BEARING_INC == 0, "check 360 is a multiple of OA_BEARING_INC");
+    static_assert(360 % OA_BENDYRULER_BEARING_INC_XY == 0, "check 360 is a multiple of OA_BEARING_INC");
 
     // search in OA_BENDYRULER_BEARING_INC degree increments around the vehicle alternating left
     // and right. For each direction check if vehicle would avoid all obstacles
@@ -108,14 +139,14 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
     float best_margin = -FLT_MAX;
     float best_margin_bearing = best_bearing;
 
-    for (uint8_t i = 0; i <= (170 / OA_BENDYRULER_BEARING_INC); i++) {
+    for (uint8_t i = 0; i <= (170 / OA_BENDYRULER_BEARING_INC_XY); i++) {
         for (uint8_t bdir = 0; bdir <= 1; bdir++) {
             // skip duplicate check of bearing straight towards destination
             if ((i==0) && (bdir > 0)) {
                 continue;
             }
             // bearing that we are probing
-            const float bearing_delta = i * OA_BENDYRULER_BEARING_INC * (bdir == 0 ? -1.0f : 1.0f);
+            const float bearing_delta = i * OA_BENDYRULER_BEARING_INC_XY * (bdir == 0 ? -1.0f : 1.0f);
             const float bearing_test = wrap_180(bearing_to_dest + bearing_delta);
 
             // ToDo: add effective groundspeed calculations using airspeed
@@ -125,7 +156,7 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
             Location test_loc = current_loc;
             test_loc.offset_bearing(bearing_test, lookahead_step1_dist);
 
-            // calculate margin from fence for this scenario
+            // calculate margin from obstacles for this scenario
             float margin = calc_avoidance_margin(current_loc, test_loc);
             if (margin > best_margin) {
                 best_margin_bearing = bearing_test;
@@ -156,6 +187,7 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
                     float margin2 = calc_avoidance_margin(test_loc, test_loc2);
                     if (margin2 > _margin_max) {
                         // if the chosen direction is directly towards the destination avoidance can be turned off
+                        // i == 0 && j == 0 implies no deviation from bearing to destination 
                         const bool active = (i != 0 || j != 0);
                         float final_bearing = bearing_test;
                         float final_margin = margin;
@@ -166,7 +198,7 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
                         destination_new = current_loc;
                         destination_new.offset_bearing(final_bearing, distance_to_dest);
                         _current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
-                        AP::logger().Write_OABendyRuler(active, bearing_to_dest, ignore_bearing_change, final_margin, destination, destination_new);
+                        AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, active, bearing_to_dest, 0.0f, ignore_bearing_change, final_margin, destination, destination_new);
                         return active;
                     }
                 }
@@ -192,11 +224,135 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
     destination_new.offset_bearing(chosen_bearing, distance_to_dest);
 
     // log results
-    AP::logger().Write_OABendyRuler(true, chosen_bearing, false, best_margin, destination, destination_new);
+    AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, true, chosen_bearing, 0.0f, false, best_margin, destination, destination_new);
+
+    return true;
+}
+
+// Search for path in the vertical directions
+bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest) 
+{
+    // check OA_BEARING_INC_VERTICAL definition allows checking in all directions
+    static_assert(360 % OA_BENDYRULER_BEARING_INC_VERTICAL == 0, "check 360 is a multiple of OA_BEARING_INC_VERTICAL");
+    float best_pitch = 0.0f;
+    bool  have_best_pitch = false;
+    float best_margin = -FLT_MAX;
+    float best_margin_pitch = best_pitch;
+    const uint8_t angular_limit = 180 / OA_BENDYRULER_BEARING_INC_VERTICAL;
+
+    for (uint8_t i = 0; i <= angular_limit; i++) {
+        for (uint8_t bdir = 0; bdir <= 1; bdir++) {
+            // skip duplicate check of bearing straight towards destination or 180 degrees behind
+            if (((i==0) && (bdir > 0)) || ((i == angular_limit) && (bdir > 0))) {
+                continue;
+            }
+
+            // bearing that we are probing
+            const float pitch_delta = i * OA_BENDYRULER_BEARING_INC_VERTICAL * (bdir == 0 ? 1.0f : -1.0f);
+
+            Location test_loc = current_loc;
+            test_loc.offset_bearing_and_pitch(bearing_to_dest, pitch_delta, lookahead_step1_dist);
+
+            // calculate margin from obstacles for this scenario
+            float margin = calc_avoidance_margin(current_loc, test_loc);
+
+            if (margin > best_margin) {
+                best_margin_pitch = pitch_delta;
+                best_margin = margin;
+            }
+
+            if (margin > _margin_max) {
+                // this path avoids the obstacles with the required margin, now check for the path ahead
+                if (!have_best_pitch) {
+                    best_pitch = pitch_delta;
+                    have_best_pitch = true;
+                }
+                const float test_pitch_step2[] { 0.0f, 90.0f, -90.0f, 180.0f};
+                float bearing_to_dest2;
+                if (is_equal(fabsf(pitch_delta), 90.0f)) {
+                    bearing_to_dest2 = bearing_to_dest; 
+                } else { 
+                    bearing_to_dest2 = test_loc.get_bearing_to(destination) * 0.01f;
+                }
+                float distance2 = constrain_float(lookahead_step2_dist, OA_BENDYRULER_LOOKAHEAD_STEP2_MIN, test_loc.get_distance(destination));
+
+                for (uint8_t j = 0; j < ARRAY_SIZE(test_pitch_step2); j++) {
+                    float bearing_test2 = wrap_180(test_pitch_step2[j]);
+                    Location test_loc2 = test_loc;
+                    test_loc2.offset_bearing_and_pitch(bearing_to_dest2, bearing_test2 ,distance2);
+
+                    // calculate minimum margin to fence and obstacles for this scenario
+                    float margin2 = calc_avoidance_margin(test_loc, test_loc2);
+                    if (margin2 > _margin_max) {
+                        // if the chosen direction is directly towards the destination we might turn off avoidance
+                        // i == 0 && j == 0 implies no deviation from bearing to destination 
+                        bool active = (i != 0 || j != 0);
+                        if (!active) {
+                            // do a sub test to confirm if we should really turn of BendyRuler
+                            const float sub_test_pitch_step2[] {-90.0f, 90.0f};
+                            for (uint8_t k = 0; k < ARRAY_SIZE(sub_test_pitch_step2); k++) {
+                                Location test_loc_sub_test = test_loc;
+                                test_loc_sub_test.offset_bearing_and_pitch(bearing_to_dest2, sub_test_pitch_step2[k], _margin_max);
+                                float margin_sub_test = calc_avoidance_margin(test_loc, test_loc_sub_test);
+                                if (margin_sub_test < _margin_max) {
+                                    // BendyRuler will remain active
+                                    active = true;
+                                    break;
+                                }
+                            }
+                        }
+                        // project in the chosen direction by the full distance
+                        destination_new = current_loc;
+                        destination_new.offset_bearing_and_pitch(bearing_to_dest,pitch_delta, distance_to_dest);
+                        _current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
+                    
+                        AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new);
+                        return active;
+                    }
+                }
+            }
+        }        
+    }   
+
+    float chosen_pitch;
+    if (have_best_pitch) {
+        // none of the directions tested were OK for 2-step checks. Choose the direction
+        // that was best for the first step
+        chosen_pitch = best_pitch;
+        _current_lookahead = MIN(_lookahead, _current_lookahead * 1.05f);
+    } else {
+        // none of the possible paths had a positive margin. Choose
+        // the one with the highest margin
+        chosen_pitch = best_margin_pitch;
+        _current_lookahead = MAX(_lookahead * 0.5f, _current_lookahead * 0.9f);
+    }
+
+    // calculate new target based on best effort
+    destination_new = current_loc;
+    destination_new.offset_bearing_and_pitch(bearing_to_dest, chosen_pitch, distance_to_dest);
+
+    // log results
+    AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, true, bearing_to_dest, chosen_pitch,false, best_margin, destination, destination_new);
 
     return true;
 }
 
+AP_OABendyRuler::OABendyType AP_OABendyRuler::get_type() const
+{
+    switch (_bendy_type) {
+        case (uint8_t)OABendyType::OA_BENDY_VERTICAL:
+        #if VERTICAL_ENABLED 
+            return OABendyType::OA_BENDY_VERTICAL;
+        #endif
+
+        case (uint8_t)OABendyType::OA_BENDY_HORIZONTAL:
+        default:
+            return OABendyType::OA_BENDY_HORIZONTAL;
+    }
+    // should never reach here
+    return OABendyType::OA_BENDY_HORIZONTAL;
+}
+
 /*
 This function is called when BendyRuler has found a bearing which is obstacles free at atleast lookahead_step1_dist and  then lookahead_step2_dist from the present location
 In many situations, this new bearing can be either left or right of the obstacle, and BendyRuler can have a tough time deciding between the two.
@@ -253,6 +409,15 @@ float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Locati
         margin_min = MIN(margin_min, latest_margin);
     }
     
+    #if VERTICAL_ENABLED 
+    // alt fence only is only needed in vertical avoidance
+    if (get_type() == OABendyType::OA_BENDY_VERTICAL) {
+        if (calc_margin_from_alt_fence(start, end, latest_margin)) {
+            margin_min = MIN(margin_min, latest_margin);
+        }
+    }
+    #endif
+
     if (calc_margin_from_object_database(start, end, latest_margin)) {
         margin_min = MIN(margin_min, latest_margin);
     }
@@ -295,6 +460,38 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con
     return true;
 }
 
+// calculate minimum distance between a path and the altitude fence
+// on success returns true and updates margin
+bool AP_OABendyRuler::calc_margin_from_alt_fence(const Location &start, const Location &end, float &margin) const
+{   
+    // exit immediately if polygon fence is not enabled
+    const AC_Fence *fence = AC_Fence::get_singleton();
+    if (fence == nullptr) {
+        return false;
+    }
+    if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) == 0) {
+        return false;
+    }
+
+    int32_t alt_above_home_cm_start, alt_above_home_cm_end;    
+    if (!start.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm_start)) {
+        return false;
+    }
+    if (!end.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm_end )) {
+        return false;
+    }
+
+    // safe max alt = fence alt - fence margin
+    const float max_fence_alt = fence->get_safe_alt_max();
+    const float margin_start =  max_fence_alt - alt_above_home_cm_start * 0.01f;
+    const float margin_end =  max_fence_alt - alt_above_home_cm_end * 0.01f;
+
+    // margin is minimum distance to fence from either start or end location
+    margin = MIN(margin_start,margin_end);
+
+    return true;
+}
+
 // calculate minimum distance between a path and all inclusion and exclusion polygons
 // on success returns true and updates margin
 bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const

libraries/AC_Avoidance/AP_OABendyRuler.h

@@ -23,6 +23,21 @@ public:
     // returns true and populates origin_new and destination_new if OA is required.  returns false if OA is not required
     bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new);
 
+    enum class OABendyType {
+        OA_BENDY_DISABLED   = 0,
+        OA_BENDY_HORIZONTAL = 1,
+        OA_BENDY_VERTICAL   = 2,
+    };
+
+    // return type of BendyRuler in use
+    OABendyType get_type() const;
+
+    // search for path in XY direction
+    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);
+
+    // search for path in the Vertical directions
+    bool search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest); 
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -37,6 +52,10 @@ private:
     // on success returns true and updates margin
     bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const;
 
+    // calculate minimum distance between a path and the altitude fence
+    // on success returns true and updates margin
+    bool calc_margin_from_alt_fence(const Location &start, const Location &end, float &margin) const;
+
     // calculate minimum distance between a path and all inclusion and exclusion polygons
     // on success returns true and updates margin
     bool calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const;
@@ -56,6 +75,7 @@ private:
     AP_Float _lookahead;            // object avoidance will look this many meters ahead of vehicle
     AP_Float _bendy_ratio;          // object avoidance will avoid major directional change if change in margin ratio is less than this param
     AP_Int16 _bendy_angle;          // object avoidance will try avoding change in direction over this much angle
+    AP_Int8  _bendy_type;           // Type of BendyRuler to run
     
     // internal variables used by background thread
     float _current_lookahead;       // distance (in meters) ahead of the vehicle we are looking for obstacles

libraries/AC_Avoidance/AP_OAPathPlanner.cpp

@@ -107,6 +107,15 @@ void AP_OAPathPlanner::init()
     start_thread();
 }
 
+// return type of BendyRuler in use
+AP_OABendyRuler:: OABendyType AP_OAPathPlanner::get_bendy_type() const
+{
+    if (_oabendyruler == nullptr) {
+        return AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED; 
+    }     
+    return _oabendyruler->get_type();
+}
+
 // pre-arm checks that algorithms have been initialised successfully
 bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
 {

libraries/AC_Avoidance/AP_OAPathPlanner.h

@@ -62,6 +62,9 @@ public:
 
     uint16_t get_options() const { return _options;}
 
+    // helper function to return type of BendyRuler in use. This is used by AC_WPNav_OA
+    AP_OABendyRuler::OABendyType get_bendy_type() const;
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private: