AC_WPNav_OA: separate handlers for results from Dijkstra's and BendyRuler

also bendy ruler uses pos controller

libraries/AC_WPNav/AC_WPNav_OA.cpp

@@ -1,3 +1,5 @@
+#include <AP_Math/control.h>
+#include <AP_InternalError/AP_InternalError.h>
 #include "AC_WPNav_OA.h"
 
 AC_WPNav_OA::AC_WPNav_OA(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) :
@@ -83,7 +85,8 @@ bool AC_WPNav_OA::update_wpnav()
         const Location origin_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
         const Location destination_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
         Location oa_origin_new, oa_destination_new;
-        const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, origin_loc, destination_loc, oa_origin_new, oa_destination_new);
+        AP_OAPathPlanner::OAPathPlannerUsed path_planner_used = AP_OAPathPlanner::OAPathPlannerUsed::None;
+        const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, origin_loc, destination_loc, oa_origin_new, oa_destination_new, path_planner_used);
         switch (oa_retstate) {
         case AP_OAPathPlanner::OA_NOT_REQUIRED:
             if (_oa_state != oa_retstate) {
@@ -107,26 +110,94 @@ bool AC_WPNav_OA::update_wpnav()
             }
             break;
         case AP_OAPathPlanner::OA_SUCCESS:
-            // if oa destination has become active or destination has changed update wpnav
+
+            // handling of returned destination depends upon path planner used
+            switch (path_planner_used) {
+
+            case AP_OAPathPlanner::OAPathPlannerUsed::None:
+                // this should never happen.  this means the path planner has returned success but has failed to set the path planner used
+                INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+                return false;
+
+            case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras:
+                // Dijkstra's.  Action is only needed if path planner has just became active or the target destination's lat or lon has changed
                 if ((_oa_state != AP_OAPathPlanner::OA_SUCCESS) || !oa_destination_new.same_latlon_as(_oa_destination)) {
-                _oa_destination = oa_destination_new;
+                    Location origin_oabak_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
-                // convert Location to offset from EKF origin
+                    Location destination_oabak_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
-                Vector3f dest_NEU;
+                    oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc);
-                if (_oa_destination.get_vector_from_origin_NEU(dest_NEU)) {
+                    if (!set_wp_destination_loc(oa_destination_new)) {
-                    if ((oa_ptr->get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL) ||
+                        // trigger terrain failsafe
-                        (oa_ptr->get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED)) {
+                        return false;
-                        // 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_oabak)) {
+                    // if new target set successfully, update oa state and destination
                     _oa_state = oa_retstate;
-                    }
+                    _oa_destination = oa_destination_new;
-                }
                 }
                 break;
+
+            case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerHorizontal: {
+                _oa_state = oa_retstate;
+                _oa_destination = oa_destination_new;
+
+                // altitude target interpolated from current_loc's distance along the original path
+                Location target_alt_loc = current_loc;
+                target_alt_loc.linearly_interpolate_alt(origin_loc, destination_loc);
+
+                // correct target_alt_loc's alt-above-ekf-origin if using terrain altitudes
+                // positive terr_offset means terrain below vehicle is above ekf origin's altitude
+                float terr_offset = 0;
+                if (_terrain_alt_oabak && !get_terrain_offset(terr_offset)) {
+                    // trigger terrain failsafe
+                    return false;
                 }
+
+                // calculate final destination as an offset from EKF origin in NEU
+                Vector2f dest_NE;
+                if (!_oa_destination.get_vector_xy_from_origin_NE(dest_NE)) {
+                    // this should never happen because we can only get here if we have an EKF origin
+                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+                    return false;
+                }
+                Vector3p dest_NEU{dest_NE.x, dest_NE.y, (float)target_alt_loc.alt};
+
+                // pass the desired position directly to the position controller
+                _pos_control.input_pos_xyz(dest_NEU, terr_offset, 1000.0);
+
+                // update horizontal position controller (vertical is updated in vehicle code)
+                _pos_control.update_xy_controller();
+
+                // return success without calling parent AC_WPNav
+                return true;
+            }
+
+            case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerVertical:
+            {
+                _oa_state = oa_retstate;
+                _oa_destination = oa_destination_new;
+
+                // calculate final destination as an offset from EKF origin in NEU
+                Vector3f dest_NEU;
+                if (!_oa_destination.get_vector_from_origin_NEU(dest_NEU)) {
+                    // this should never happen because we can only get here if we have an EKF origin
+                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+                    return false;
+                }
+
+                // pass the desired position directly to the position controller as an offset from EKF origin in NEU
+                Vector3p dest_NEU_p{dest_NEU.x, dest_NEU.y, dest_NEU.z};
+                _pos_control.input_pos_xyz(dest_NEU_p, 0, 1000.0);
+
+                // update horizontal position controller (vertical is updated in vehicle code)
+                _pos_control.update_xy_controller();
+
+                // Note: do not update yaw or yaw rate as we do for BendyRuler horizontal
+
+                // return success without calling parent AC_WPNav
+                return true;
+            }
+
+            } // case AP_OAPathPlanner::OA_SUCCESS
+        } // switch (oa_retstate)
     }
 
     // run the non-OA update