AC_Avoid: Extend BendyRuler functionality to search for vertical paths - Try 2

libraries/AC_Avoidance/AP_OABendyRuler.h

@@ -23,6 +23,21 @@ class AP_OABendyRuler {
     // 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 @@ class AP_OABendyRuler {
     // 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,7 +75,8 @@ class AP_OABendyRuler {
     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
     float _bearing_prev;            // stored bearing in degrees 

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 @@ class AP_OAPathPlanner {
 
     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:

libraries/AC_WPNav/AC_WPNav_OA.cpp

@@ -112,10 +112,12 @@ bool AC_WPNav_OA::update_wpnav()
                 // convert Location to offset from EKF origin
                 Vector3f dest_NEU;
                 if (_oa_destination.get_vector_from_origin_NEU(dest_NEU)) {
-                    // calculate target altitude by calculating OA adjusted destination's distance along the original track
-                    // and then linear interpolate using the original track's origin and destination altitude
-                    const float dist_along_path = constrain_float(oa_destination_new.line_path_proportion(origin_loc, destination_loc), 0.0f, 1.0f);
-                    dest_NEU.z = linear_interpolate(_origin_oabak.z, _destination_oabak.z, dist_along_path, 0.0f, 1.0f);
+                    if (oa_ptr -> get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL || oa_ptr -> get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED) {
+                        // calculate target altitude by calculating OA adjusted destination's distance along the original track
+                        // and then linear interpolate using the original track's origin and destination altitude
+                        const float dist_along_path = constrain_float(oa_destination_new.line_path_proportion(origin_loc, destination_loc), 0.0f, 1.0f);
+                        dest_NEU.z = linear_interpolate(_origin_oabak.z, _destination_oabak.z, dist_along_path, 0.0f, 1.0f);
+                    }       
                     if (set_wp_destination(dest_NEU, _terrain_alt)) {
                         _oa_state = oa_retstate;
                     }

libraries/AC_WPNav/AC_WPNav_OA.h

@@ -2,6 +2,7 @@
 
 #include <AC_WPNav/AC_WPNav.h>
 #include <AC_Avoidance/AP_OAPathPlanner.h>
+#include <AC_Avoidance/AP_OABendyRuler.h>
 
 class AC_WPNav_OA : public AC_WPNav
 {

libraries/AP_Common/Location.cpp

@@ -265,6 +265,17 @@ void Location::offset_bearing(float bearing, float distance)
     offset(ofs_north, ofs_east);
 }
 
+// extrapolate latitude/longitude given bearing, pitch and distance
+void Location::offset_bearing_and_pitch(float bearing, float pitch, float distance)
+{
+    const float ofs_north =  cosf(radians(pitch)) * cosf(radians(bearing)) * distance;
+    const float ofs_east  =  cosf(radians(pitch)) * sinf(radians(bearing)) * distance;
+    offset(ofs_north, ofs_east);
+    const int32_t dalt =  sinf(radians(pitch)) * distance *100.0f;
+    alt += dalt; 
+}
+
+
 float Location::longitude_scale() const
 {
     float scale = cosf(lat * (1.0e-7f * DEG_TO_RAD));

libraries/AP_Common/Location.h

@@ -73,6 +73,9 @@ class Location
 
     // extrapolate latitude/longitude given bearing and distance
     void offset_bearing(float bearing, float distance);
+
+    // extrapolate latitude/longitude given bearing, pitch and distance
+    void offset_bearing_and_pitch(float bearing, float pitch, float distance);
 
     // longitude_scale - returns the scaler to compensate for
     // shrinking longitude as you move north or south from the equator

libraries/AP_Logger/AP_Logger.h

@@ -285,7 +285,7 @@ class AP_Logger
     void Write_Beacon(AP_Beacon &beacon);
     void Write_Proximity(AP_Proximity &proximity);
     void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);
-    void Write_OABendyRuler(bool active, float target_yaw, bool ignore_chg, float margin, const Location &final_dest, const Location &oa_dest);
+    void Write_OABendyRuler(uint8_t type, bool active, float target_yaw, float target_pitch, bool ignore_chg, float margin, const Location &final_dest, const Location &oa_dest);
     void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest);
     void Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up);
     void Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp);

libraries/AP_Logger/LogFile.cpp

@@ -1044,20 +1044,24 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points
     WriteBlock(&pkt_srtl, sizeof(pkt_srtl));
 }
 
-void AP_Logger::Write_OABendyRuler(bool active, float target_yaw, bool resist_chg, float margin, const Location &final_dest, const Location &oa_dest)
+void AP_Logger::Write_OABendyRuler(uint8_t type, bool active, float target_yaw, float target_pitch, bool resist_chg, float margin, const Location &final_dest, const Location &oa_dest)
 {
     const struct log_OABendyRuler pkt{
         LOG_PACKET_HEADER_INIT(LOG_OA_BENDYRULER_MSG),
         time_us     : AP_HAL::micros64(),
+        type        : type,
         active      : active,
         target_yaw  : (uint16_t)wrap_360(target_yaw),
         yaw         : (uint16_t)wrap_360(AP::ahrs().yaw_sensor * 0.01f),
+        target_pitch: (uint16_t)target_pitch,
         resist_chg  : resist_chg,
         margin      : margin,
         final_lat   : final_dest.lat,
         final_lng   : final_dest.lng,
+        final_alt   : final_dest.alt,
         oa_lat      : oa_dest.lat,
-        oa_lng      : oa_dest.lng
+        oa_lng      : oa_dest.lng,
+        oa_alt      : oa_dest.alt
     };
     WriteBlock(&pkt, sizeof(pkt));
 }

libraries/AP_Logger/LogStructure.h

@@ -1170,15 +1170,19 @@ struct PACKED log_SRTL {
 struct PACKED log_OABendyRuler {
     LOG_PACKET_HEADER;
     uint64_t time_us;
+    uint8_t type;
     uint8_t active;
     uint16_t target_yaw;
     uint16_t yaw;
+    uint16_t target_pitch;
     bool resist_chg;
     float margin;
     int32_t final_lat;
     int32_t final_lng;
+    int32_t final_alt;
     int32_t oa_lat;
     int32_t oa_lng;
+    int32_t oa_alt;
 };
 
 struct PACKED log_OADijkstra {
@@ -1910,15 +1914,19 @@ struct PACKED log_Winch {
 // @LoggerMessage: OABR
 // @Description: Object avoidance (Bendy Ruler) diagnostics
 // @Field: TimeUS: Time since system startup
-// @Field: Active: True if Bendy Ruler avoidance is being used
-// @Field: DesYaw: Best yaw chosen to avoid obstacle
+// @Field: Type: Type of BendyRuler currently active
+// @Field: Act: True if Bendy Ruler avoidance is being used
+// @Field: DYaw: Best yaw chosen to avoid obstacle
 // @Field: Yaw: Current vehicle yaw
-// @Field: ResChg: True if BendyRuler resisted changing bearing and continued in last calculated bearing
+// @Field: DP: Desired pitch chosen to avoid obstacle
+// @Field: RChg: True if BendyRuler resisted changing bearing and continued in last calculated bearing
 // @Field: Mar: Margin from path to obstacle on best yaw chosen
-// @Field: DLat: Destination latitude
-// @Field: DLng: Destination longitude
-// @Field: OALat: Intermediate location chosen for avoidance
-// @Field: OALng: Intermediate location chosen for avoidance
+// @Field: DLt: Destination latitude
+// @Field: DLg: Destination longitude
+// @Field: DAlt: Desired alt
+// @Field: OLt: Intermediate location chosen for avoidance
+// @Field: OLg: Intermediate location chosen for avoidance
+// @Field: OAlt: Intermediate alt chosen for avoidance
 
 // @LoggerMessage: OADJ
 // @Description: Object avoidance (Dijkstra) diagnostics
@@ -2485,7 +2493,7 @@ struct PACKED log_Winch {
     { LOG_SRTL_MSG, sizeof(log_SRTL), \
       "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \
     { LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \
-      "OABR","QBHHBfLLLL","TimeUS,Active,DesYaw,Yaw,ResChg,Mar,DLat,DLng,OALat,OALng", "sbdd-mDUDU", "F-----GGGG" }, \
+      "OABR","QBBHHHBfLLfLLf","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s-bddd-mDUmDUm", "F-------GGBGGB" }, \
     { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
       "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \
     { LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \