Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include <AP_AHRS/AP_AHRS.h>
      17             : #include <AP_Math/AP_Math.h>
      18             : #include <AP_HAL/AP_HAL.h>
      19             : #include "AR_WPNav_OA.h"
      20             : #include <GCS_MAVLink/GCS.h>
      21             : #include <AP_InternalError/AP_InternalError.h>
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : // update navigation
      26      180861 : void AR_WPNav_OA::update(float dt)
      27             : {
      28             : #if AP_OAPATHPLANNER_ENABLED
      29             :     // exit immediately if no current location, origin or destination
      30      180861 :     Location current_loc;
      31             :     float speed;
      32      180861 :     if (!hal.util->get_soft_armed() || !is_destination_valid() || !AP::ahrs().get_location(current_loc) || !_atc.get_forward_speed(speed)) {
      33       10345 :         _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
      34       10345 :         _desired_lat_accel = 0.0f;
      35       10345 :         _desired_turn_rate_rads = 0.0f;
      36       10345 :         _oa_active = false;
      37       10490 :         return;
      38             :     }
      39             : 
      40             :     // run path planning around obstacles
      41      170516 :     bool stop_vehicle = false;
      42             : 
      43             :     // backup _origin, _destination and _next_destination when not doing oa
      44      170516 :     if (!_oa_active) {
      45      149006 :         _origin_oabak = _origin;
      46      149006 :         _destination_oabak = _destination;
      47      149006 :         _next_destination_oabak = _next_destination;
      48             :     }
      49             : 
      50      170516 :     AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton();
      51      170516 :     if (oa != nullptr) {
      52      170516 :         Location oa_origin_new, oa_destination_new, oa_next_destination_new;
      53             :         AP_OAPathPlanner::OAPathPlannerUsed path_planner_used;
      54             :         bool dest_to_next_dest_clear;
      55      170516 :         const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc,
      56      170516 :                                                                                 _origin_oabak,
      57      170516 :                                                                                 _destination_oabak,
      58      170516 :                                                                                 _next_destination_oabak,
      59             :                                                                                 oa_origin_new,
      60             :                                                                                 oa_destination_new,
      61             :                                                                                 oa_next_destination_new,
      62             :                                                                                 dest_to_next_dest_clear,
      63             :                                                                                 path_planner_used);
      64             : 
      65      170516 :         switch (oa_retstate) {
      66             : 
      67      148861 :         case AP_OAPathPlanner::OA_NOT_REQUIRED:
      68      148861 :             if (_oa_active) {
      69             :                 // object avoidance has become inactive so reset target to original destination
      70           6 :                 if (!AR_WPNav::set_desired_location(_destination_oabak)) {
      71             :                     // this should never happen because we should have an EKF origin and the destination must be valid
      72           0 :                     INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      73           0 :                     stop_vehicle = true;
      74             :                 }
      75           6 :                 _oa_active = false;
      76             :                 // ToDo: handle "if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET)"
      77             :             }
      78      148861 :             break;
      79             : 
      80         145 :         case AP_OAPathPlanner::OA_PROCESSING:
      81             :         case AP_OAPathPlanner::OA_ERROR:
      82             :             // during processing or in case of error, slow vehicle to a stop
      83         145 :             stop_vehicle = true;
      84         145 :             _oa_active = false;
      85         145 :             break;
      86             : 
      87       21510 :         case AP_OAPathPlanner::OA_SUCCESS:
      88             :             // handling of returned destination depends upon path planner used
      89       21510 :             switch (path_planner_used) {
      90             : 
      91           0 :             case AP_OAPathPlanner::OAPathPlannerUsed::None:
      92             :             case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerVertical:
      93             :                 // this should never happen.  this means the path planner has returned success but has returned an invalid planner
      94           0 :                 INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      95           0 :                 _oa_active = false;
      96           0 :                 stop_vehicle = true;
      97           0 :                 return;
      98             : 
      99       11461 :             case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras:
     100             :                 // Dijkstra's.  Action is only needed if path planner has just became active or the target destination's lat or lon has changed
     101       11461 :                 if (!_oa_active || !oa_destination_new.same_latlon_as(_oa_destination)) {
     102          10 :                     if (AR_WPNav::set_desired_location(oa_destination_new)) {
     103             :                         // if new target set successfully, update oa state and destination
     104          10 :                         _oa_active = true;
     105          10 :                         _oa_origin = oa_origin_new;
     106          10 :                         _oa_destination = oa_destination_new;
     107             :                     } else {
     108             :                         // this should never happen
     109           0 :                         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     110           0 :                         stop_vehicle = true;
     111             :                     }
     112             :                 }
     113       11461 :                 break;
     114             : 
     115       10049 :             case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerHorizontal: {
     116             :                 // BendyRuler.  Action is only needed if path planner has just became active or the target destination's lat or lon has changed
     117       10049 :                 if (!_oa_active || !oa_destination_new.same_latlon_as(_oa_destination)) {
     118         199 :                     if (AR_WPNav::set_desired_location_expect_fast_update(oa_destination_new)) {
     119             :                         // if new target set successfully, update oa state and destination
     120         199 :                         _oa_active = true;
     121         199 :                         _oa_origin = oa_origin_new;
     122         199 :                         _oa_destination = oa_destination_new;
     123             :                     } else {
     124             :                         // this should never happen
     125           0 :                         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     126           0 :                         stop_vehicle = true;
     127             :                     }
     128             :                 }
     129             :             }
     130       10049 :             break;
     131             : 
     132             :             } // switch (path_planner_used) {
     133             :         } // switch (oa_retstate) {
     134             :     } // if (oa != nullptr) {
     135             : 
     136      170516 :     update_oa_distance_and_bearing_to_destination();
     137             : 
     138             :     // handle stopping vehicle if avoidance has failed
     139      170516 :     if (stop_vehicle) {
     140             :         // decelerate to speed to zero and set turn rate to zero
     141         145 :         _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
     142         145 :         _desired_lat_accel = 0.0f;
     143         145 :         _desired_turn_rate_rads = 0.0f;
     144         145 :         return;
     145             :     }
     146             : #endif  // AP_OAPATHPLANNER_ENABLED
     147             : 
     148             :     // call parent update
     149      170371 :     AR_WPNav::update(dt);
     150             : }
     151             : 
     152             : // set desired location and (optionally) next_destination
     153             : // next_destination should be provided if known to allow smooth cornering
     154         184 : bool AR_WPNav_OA::set_desired_location(const Location& destination, Location next_destination)
     155             : {
     156         184 :     const bool ret = AR_WPNav::set_desired_location(destination, next_destination);
     157             : 
     158         184 :     if (ret) {
     159             :         // disable object avoidance, it will be re-enabled (if necessary) on next update
     160         184 :         _oa_active = false;
     161             :     }
     162             : 
     163         184 :     return ret;
     164             : }
     165             : 
     166             : // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
     167      229165 : bool AR_WPNav_OA::reached_destination() const
     168             : {
     169             :     // object avoidance should always be deactivated before reaching final destination
     170      229165 :     if (_oa_active) {
     171       29436 :         return false;
     172             :     }
     173             : 
     174      199729 :     return AR_WPNav::reached_destination();
     175             : }
     176             : 
     177             : // get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
     178      170371 : const Location &AR_WPNav_OA::get_oa_origin() const
     179             : {
     180      170371 :     if (_oa_active) {
     181       21510 :         return _oa_origin;
     182             :     }
     183             : 
     184      148861 :     return _origin;
     185             : }
     186             : 
     187             : // get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
     188      206285 : const Location &AR_WPNav_OA::get_oa_destination() const
     189             : {
     190      206285 :     if (_oa_active) {
     191       25812 :         return _oa_destination;
     192             :     }
     193             : 
     194      180473 :     return AR_WPNav::get_oa_destination();
     195             : }
     196             : 
     197             : // return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats)
     198      181534 : float AR_WPNav_OA::oa_wp_bearing_cd() const
     199             : {
     200      181534 :     if (_oa_active) {
     201       21718 :         return _oa_wp_bearing_cd;
     202             :     }
     203             : 
     204      159816 :     return AR_WPNav::oa_wp_bearing_cd();
     205             : }
     206             : 
     207             : // update distance from vehicle's current position to destination
     208      170516 : void AR_WPNav_OA::update_oa_distance_and_bearing_to_destination()
     209             : {
     210             :     // update OA adjusted values
     211      170516 :     Location current_loc;
     212      170516 :     if (_oa_active && AP::ahrs().get_location(current_loc)) {
     213       21510 :         _oa_distance_to_destination = current_loc.get_distance(_oa_destination);
     214       21510 :         _oa_wp_bearing_cd = current_loc.get_bearing_to(_oa_destination);
     215             :     } else {
     216      149006 :         _oa_distance_to_destination = AR_WPNav::get_distance_to_destination();
     217      149006 :         _oa_wp_bearing_cd = AR_WPNav::wp_bearing_cd();
     218             :     }
     219      170516 : }