From 3242bd0ee89db6bf9852e8a9ec4340798e2151b2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Feb 2021 14:03:07 +0900 Subject: [PATCH] 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 | 107 ++++++++++++++++++++++++----- 1 file changed, 89 insertions(+), 18 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index cd400e70e7..8f90301f36 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -1,3 +1,5 @@ +#include +#include #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 - if ((_oa_state != AP_OAPathPlanner::OA_SUCCESS) || !oa_destination_new.same_latlon_as(_oa_destination)) { - _oa_destination = oa_destination_new; - // convert Location to offset from EKF origin - Vector3f dest_NEU; - if (_oa_destination.get_vector_from_origin_NEU(dest_NEU)) { - 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_oabak)) { - _oa_state = oa_retstate; + + // 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)) { + Location origin_oabak_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + Location destination_oabak_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); + if (!set_wp_destination_loc(oa_destination_new)) { + // trigger terrain failsafe + return false; } + // 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; } - break; - } + + 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