Line data    Source code

       1             : #include "AC_WPNav_config.h"
       2             : 
       3             : #if AC_WPNAV_OA_ENABLED
       4             : 
       5             : #include <AP_Math/control.h>
       6             : #include <AP_InternalError/AP_InternalError.h>
       7             : #include "AC_WPNav_OA.h"
       8             : 
       9        1001 : AC_WPNav_OA::AC_WPNav_OA(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) :
      10        1001 :     AC_WPNav(ahrs, pos_control, attitude_control)
      11             : {
      12        1001 : }
      13             : 
      14             : // Returns the object-avoidance-adjusted waypoint location (in global coordinates).
      15             : // Falls back to original destination if OA is not active.
      16       45572 : bool AC_WPNav_OA::get_oa_wp_destination(Location& destination) const
      17             : {
      18             :     // Return unmodified global destination if OA is not active
      19       45572 :     if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
      20       45572 :         return get_wp_destination_loc(destination);
      21             :     }
      22             : 
      23             :     // OA is active — return path-planner-adjusted intermediate destination
      24           0 :     destination = _oa_destination;
      25           0 :     return true;
      26             : }
      27             : 
      28             : // Sets the waypoint destination using NEU coordinates in centimeters.
      29             : // See set_wp_destination_NED_m() for full details.
      30           0 : bool AC_WPNav_OA::set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt)
      31             : {
      32             :     // Convert input from NEU centimeters to NED meters and delegate to meter version
      33           0 :     Vector3p destination_ned_m = Vector3p(destination_neu_cm.x, destination_neu_cm.y, -destination_neu_cm.z) * 0.01;
      34           0 :     return set_wp_destination_NED_m(destination_ned_m, is_terrain_alt);
      35             : }
      36             : 
      37             : // Sets the waypoint destination using NED coordinates in meters.
      38             : // - destination_ned_m: NED offset from EKF origin in meters.
      39             : // - is_terrain_alt: true if the destination_ned_m is relative to the terrain surface.
      40             : // - Resets OA state on success.
      41         888 : bool AC_WPNav_OA::set_wp_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, float arc_ang_rad)
      42             : {
      43             :     // Call base implementation to set destination and terrain-altitude flag
      44         888 :     const bool ret = AC_WPNav::set_wp_destination_NED_m(destination_ned_m, is_terrain_alt, arc_ang_rad);
      45             : 
      46             :     // If destination set successfully, reset OA state to inactive
      47         888 :     if (ret) {
      48             :         // reset object avoidance state
      49         888 :         _oa_state = AP_OAPathPlanner::OA_NOT_REQUIRED;
      50             :     }
      51             : 
      52         888 :     return ret;
      53             : }
      54             : 
      55             : // Returns the horizontal distance to the final destination in centimeters.
      56             : // See get_wp_distance_to_destination_m() for full details.
      57           0 : float AC_WPNav_OA::get_wp_distance_to_destination_cm() const
      58             : {
      59             :     // Convert horizontal distance from meters to centimeters
      60           0 :     return get_wp_distance_to_destination_m() * 100.0;
      61             : }
      62             : 
      63             : // Returns the horizontal distance to the final destination in meters.
      64             : // Ignores OA-adjusted targets and always measures to the original final destination.
      65      130461 : float AC_WPNav_OA::get_wp_distance_to_destination_m() const
      66             : {
      67             :     // Return horizontal distance to final destination (ignoring OA intermediate goals)
      68      130461 :     if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
      69      130461 :         return AC_WPNav::get_wp_distance_to_destination_m();
      70             :     }
      71             : 
      72             :     // Compute distance to original destination using backed-up NED position
      73           0 :     return get_horizontal_distance(_pos_control.get_pos_estimate_NED_m().xy(), _destination_oabak_ned_m.xy());
      74             : }
      75             : 
      76             : // Returns the bearing to the final destination in centidegrees.
      77             : // See get_wp_bearing_to_destination_rad() for full details.
      78           0 : int32_t AC_WPNav_OA::get_wp_bearing_to_destination_cd() const
      79             : {
      80             :     // Convert bearing to destination (in radians) to centidegrees
      81           0 :     return rad_to_cd(get_wp_bearing_to_destination_rad());
      82             : }
      83             : 
      84             : // Returns the bearing to the final destination in radians.
      85             : // Ignores OA-adjusted targets and always calculates from original final destination.
      86      126811 : float AC_WPNav_OA::get_wp_bearing_to_destination_rad() const
      87             : {
      88             :     // Use base class method if object avoidance is inactive
      89      126811 :     if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
      90      126811 :         return AC_WPNav::get_wp_bearing_to_destination_rad();
      91             :     }
      92             : 
      93             :     // Return bearing to the original destination, not the OA-adjusted one
      94           0 :     return get_bearing_rad(_pos_control.get_pos_estimate_NED_m().xy().tofloat(), _destination_oabak_ned_m.xy().tofloat());
      95             : }
      96             : 
      97             : // Returns true if the vehicle has reached the final destination within radius threshold.
      98             : // Ignores OA-adjusted intermediate destinations.
      99     2881468 : bool AC_WPNav_OA::reached_wp_destination() const
     100             : {
     101             :     // Only consider the waypoint reached if OA is inactive and base class condition is met
     102     2881468 :     return (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) && AC_WPNav::reached_wp_destination();
     103             : }
     104             : 
     105             : // Runs the waypoint navigation update loop, including OA path planning logic.
     106             : // Delegates to parent class if the leg is a circular orbit, or if OA is not active or not required.
     107     3493840 : bool AC_WPNav_OA::update_wpnav()
     108             : {
     109             :     // Object avoidance does not run on a circular orbit leg.  The path planners are given a
     110             :     // straight origin-to-destination segment, which for an orbit is the chord back to the entry
     111             :     // point rather than the arc being flown, so they would both miss obstacles on the arc and,
     112             :     // on deactivating, rebuild the leg as a straight line to the entry point, discarding the
     113             :     // turns not yet flown.  Holding the state inactive keeps every other override here falling
     114             :     // through to the base class for the whole orbit, in particular reached_wp_destination(),
     115             :     // which would otherwise never report the orbit complete
     116     3493840 :     if (_this_leg_is_circle) {
     117       46670 :         _oa_state = AP_OAPathPlanner::OA_NOT_REQUIRED;
     118       46670 :         return AC_WPNav::update_wpnav();
     119             :     }
     120             : 
     121             :     // Run path planning logic using the active OA planner
     122     3447170 :     AP_OAPathPlanner *oa_ptr = AP_OAPathPlanner::get_singleton();
     123     3447170 :     Location current_loc;
     124     3447170 :     if ((oa_ptr != nullptr) && AP::ahrs().get_location(current_loc)) {
     125             : 
     126             :         // Backup current path state before OA modifies it
     127     3446059 :         if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
     128     3446059 :             _origin_oabak_ned_m = _origin_ned_m;
     129     3446059 :             _destination_oabak_ned_m = _destination_ned_m;
     130     3446059 :             _is_terrain_alt_oabak = _is_terrain_alt;
     131     3446059 :             _next_destination_oabak_ned_m = _next_destination_ned_m;
     132             :         }
     133             : 
     134             :         // Convert backup path state to global Location objects for planner input
     135     3446059 :         const Location origin_loc = Location::from_ekf_offset_NED_m(_origin_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
     136     3446059 :         const Location destination_loc = Location::from_ekf_offset_NED_m(_destination_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
     137     3446059 :         const Location next_destination_loc = Location::from_ekf_offset_NED_m(_next_destination_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
     138     3446059 :         Location oa_origin_new, oa_destination_new, oa_next_destination_new;
     139     3446059 :         bool dest_to_next_dest_clear = true;
     140     3446059 :         AP_OAPathPlanner::OAPathPlannerUsed path_planner_used = AP_OAPathPlanner::OAPathPlannerUsed::None;
     141             : 
     142             :         // Request obstacle-avoidance-adjusted path from planner
     143     3446059 :         const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc,
     144             :                                                                                     origin_loc,
     145             :                                                                                     destination_loc,
     146             :                                                                                     next_destination_loc,
     147             :                                                                                     oa_origin_new,
     148             :                                                                                     oa_destination_new,
     149             :                                                                                     oa_next_destination_new,
     150             :                                                                                     dest_to_next_dest_clear,
     151             :                                                                                     path_planner_used);
     152             : 
     153     3446059 :         switch (oa_retstate) {
     154             : 
     155     3446059 :         case AP_OAPathPlanner::OA_NOT_REQUIRED:
     156             :             // OA is no longer needed — restore original destination and optionally set next
     157     3446059 :             if (_oa_state != oa_retstate) {
     158             :                 // object avoidance has become inactive so reset target to original destination
     159           0 :                 if (!set_wp_destination_NED_m(_destination_oabak_ned_m, _is_terrain_alt_oabak)) {
     160             :                     // trigger terrain failsafe
     161           0 :                     return false;
     162             :                 }
     163             : 
     164             :                 // if path from destination to next_destination is clear
     165           0 :                 if (dest_to_next_dest_clear && (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS)) {
     166             :                     // set next destination if non-zero
     167           0 :                     if (!_next_destination_oabak_ned_m.is_zero()) {
     168           0 :                         set_wp_destination_next_NED_m(_next_destination_oabak_ned_m);
     169             :                     }
     170             :                 }
     171           0 :                 _oa_state = oa_retstate;
     172             :             }
     173             : 
     174             :             // Prevent transitioning past this waypoint if path to next is unclear
     175             :             // Note that this check is run on every iteration even if the path planner is not active
     176     3446059 :             if (!dest_to_next_dest_clear) {
     177           0 :                 force_stop_at_next_wp();
     178             :             }
     179     3446059 :             break;
     180             : 
     181           0 :         case AP_OAPathPlanner::OA_PROCESSING:
     182             :             // Allow continued movement while OA path is processing if fast-waypointing is enabled
     183           0 :             if (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) {
     184             :                 // if fast waypoint option is set, proceed during processing
     185           0 :                 break;
     186             :             }
     187             :             FALLTHROUGH;
     188             : 
     189             :         case AP_OAPathPlanner::OA_ERROR:
     190             :             // OA temporarily failing — stop vehicle at current position
     191           0 :             if ((_oa_state != AP_OAPathPlanner::OA_PROCESSING) && (_oa_state != AP_OAPathPlanner::OA_ERROR)) {
     192             :                 // calculate stopping point
     193           0 :                 Vector3p stopping_point_ned_m;
     194           0 :                 get_wp_stopping_point_NED_m(stopping_point_ned_m);
     195           0 :                 _oa_destination = Location::from_ekf_offset_NED_m(stopping_point_ned_m, Location::AltFrame::ABOVE_ORIGIN);
     196           0 :                 _oa_next_destination.zero();
     197           0 :                 if (set_wp_destination_NED_m(stopping_point_ned_m, false)) {
     198           0 :                     _oa_state = oa_retstate;
     199             :                 }
     200             :             }
     201           0 :             break;
     202             : 
     203           0 :         case AP_OAPathPlanner::OA_SUCCESS:
     204             : 
     205             :             // Handle result differently depending on which OA planner was used
     206           0 :             switch (path_planner_used) {
     207             : 
     208           0 :             case AP_OAPathPlanner::OAPathPlannerUsed::None:
     209             :                 // this should never happen.  this means the path planner has returned success but has failed to set the path planner used
     210           0 :                 INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     211           0 :                 return false;
     212             : 
     213           0 :             case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras:
     214             :                 // Dijkstra's.  Action is only needed if path planner has just became active or the target destination's lat or lon has changed
     215             :                 // Interpolate altitude and set new target if different or first OA success
     216           0 :                 if ((_oa_state != AP_OAPathPlanner::OA_SUCCESS) || !oa_destination_new.same_latlon_as(_oa_destination)) {
     217           0 :                     Location origin_oabak_loc = Location::from_ekf_offset_NED_m(_origin_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
     218           0 :                     Location destination_oabak_loc = Location::from_ekf_offset_NED_m(_destination_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
     219           0 :                     oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc);
     220             : 
     221             :                     // set new OA adjusted destination
     222           0 :                     if (!set_wp_destination_loc(oa_destination_new)) {
     223             :                         // trigger terrain failsafe
     224           0 :                         return false;
     225             :                     }
     226             :                     // if new target set successfully, update oa state and destination
     227           0 :                     _oa_state = oa_retstate;
     228           0 :                     _oa_destination = oa_destination_new;
     229             : 
     230             :                     // Set next destination if provided
     231           0 :                     if ((oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) && !oa_next_destination_new.is_zero()) {
     232             :                         // calculate oa_next_destination_new's altitude using linear interpolation between original origin and destination
     233             :                         // this "next destination" is still an intermediate point between the origin and destination
     234           0 :                         oa_next_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc);
     235           0 :                         if (set_wp_destination_next_loc(oa_next_destination_new)) {
     236           0 :                             _oa_next_destination = oa_next_destination_new;
     237             :                         }
     238             :                     }
     239             :                 }
     240           0 :                 break;
     241             : 
     242           0 :             case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerHorizontal: {
     243           0 :                 _oa_state = oa_retstate;
     244           0 :                 _oa_destination = oa_destination_new;
     245             : 
     246             :                 // Adjust altitude based on current progress along the path
     247           0 :                 Location target_alt_loc = current_loc;
     248           0 :                 target_alt_loc.linearly_interpolate_alt(origin_loc, destination_loc);
     249             : 
     250             :                 // Get terrain offset if needed
     251           0 :                 float terrain_d_m = 0;
     252           0 :                 if (_is_terrain_alt_oabak && !get_terrain_D_m(terrain_d_m)) {
     253             :                     // trigger terrain failsafe
     254           0 :                     return false;
     255             :                 }
     256             : 
     257             :                 // Convert global destination to NED vector and pass directly to position controller
     258           0 :                 Vector2f destination_ne_m;
     259           0 :                 if (!_oa_destination.get_vector_xy_from_origin_NE_m(destination_ne_m)) {
     260             :                     // this should never happen because we can only get here if we have an EKF origin
     261           0 :                     INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     262           0 :                     return false;
     263             :                 }
     264           0 :                 float target_alt_loc_alt_m = 0;
     265           0 :                 UNUSED_RESULT(target_alt_loc.get_alt_m(target_alt_loc.get_alt_frame(), target_alt_loc_alt_m));
     266           0 :                 Vector3p destination_ned_m{destination_ne_m.x, destination_ne_m.y, -target_alt_loc_alt_m};
     267             : 
     268             :                 // pass the desired position directly to the position controller
     269           0 :                 _pos_control.input_pos_NED_m(destination_ned_m, terrain_d_m, 10.0);
     270             : 
     271             :                 // update horizontal position controller (vertical is updated in vehicle code)
     272           0 :                 _pos_control.NE_update_controller();
     273             : 
     274             :                 // return success without calling parent AC_WPNav
     275           0 :                 return true;
     276             :             }
     277             : 
     278           0 :             case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerVertical: {
     279           0 :                 _oa_state = oa_retstate;
     280           0 :                 _oa_destination = oa_destination_new;
     281             : 
     282             :                 // Convert final destination to NED offset and push to position controller
     283           0 :                 Vector3p destination_ned_m;
     284           0 :                 if (!_oa_destination.get_vector_from_origin_NED_m(destination_ned_m)) {
     285             :                     // this should never happen because we can only get here if we have an EKF origin
     286           0 :                     INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     287           0 :                     return false;
     288             :                 }
     289             : 
     290             :                 // pass the desired position directly to the position controller as an offset from EKF origin in NED
     291           0 :                 _pos_control.input_pos_NED_m(destination_ned_m, 0, 10.0);
     292             : 
     293             :                 // update horizontal position controller (vertical is updated in vehicle code)
     294           0 :                 _pos_control.NE_update_controller();
     295             : 
     296             :                 // return success without calling parent AC_WPNav
     297           0 :                 return true;
     298             :             }
     299             : 
     300             :             }
     301             :         }
     302             :     }
     303             : 
     304             :     // Run standard waypoint update if OA was not active or handled above
     305     3447170 :     return AC_WPNav::update_wpnav();
     306             : }
     307             : 
     308             : #endif  // AC_WPNAV_OA_ENABLED