Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AC_Avoidance_config.h"
       4             : 
       5             : #if AP_OAPATHPLANNER_ENABLED
       6             : 
       7             : #include <AP_Common/AP_Common.h>
       8             : #include <AP_Common/Location.h>
       9             : #include <AP_Param/AP_Param.h>
      10             : #include <AP_HAL/Semaphores.h>
      11             : 
      12             : #include "AP_OABendyRuler.h"
      13             : #include "AP_OADijkstra.h"
      14             : #include "AP_OADatabase.h"
      15             : 
      16             : /*
      17             :  * This class provides path planning around fence, stay-out zones and moving obstacles
      18             :  */
      19             : class AP_OAPathPlanner {
      20             : 
      21             : public:
      22             :     AP_OAPathPlanner();
      23             : 
      24             :     /* Do not allow copies */
      25             :     CLASS_NO_COPY(AP_OAPathPlanner);
      26             : 
      27             :     // get singleton instance
      28     3617686 :     static AP_OAPathPlanner *get_singleton() {
      29     3617686 :         return _singleton;
      30             :     }
      31             : 
      32             :     // perform any required initialisation
      33             :     void init();
      34             : 
      35             :     /// returns true if all pre-takeoff checks have completed successfully
      36             :     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
      37             : 
      38             :     // object avoidance processing return status enum
      39             :     enum OA_RetState : uint8_t {
      40             :         OA_NOT_REQUIRED = 0,            // object avoidance is not required
      41             :         OA_PROCESSING,                  // still calculating alternative path
      42             :         OA_ERROR,                       // error during calculation
      43             :         OA_SUCCESS                      // success
      44             :     };
      45             : 
      46             :     // path planner responsible for a particular result
      47             :     enum OAPathPlannerUsed : uint8_t {
      48             :         None = 0,
      49             :         BendyRulerHorizontal,
      50             :         BendyRulerVertical,
      51             :         Dijkstras
      52             :     };
      53             : 
      54             :     // provides an alternative target location if path planning around obstacles is required
      55             :     // returns true and updates result_origin, result_destination and result_next_destination with an intermediate path
      56             :     // result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear  (only supported by Dijkstras)
      57             :     // path_planner_used updated with which path planner produced the result
      58             :     OA_RetState mission_avoidance(const Location &current_loc,
      59             :                            const Location &origin,
      60             :                            const Location &destination,
      61             :                            const Location &next_destination,
      62             :                            Location &result_origin,
      63             :                            Location &result_destination,
      64             :                            Location &result_next_destination,
      65             :                            bool &result_dest_to_next_dest_clear,
      66             :                            OAPathPlannerUsed &path_planner_used) WARN_IF_UNUSED;
      67             : 
      68             :     // enumerations for _TYPE parameter
      69             :     enum OAPathPlanTypes {
      70             :         OA_PATHPLAN_DISABLED = 0,
      71             :         OA_PATHPLAN_BENDYRULER = 1,
      72             :         OA_PATHPLAN_DIJKSTRA = 2,
      73             :         OA_PATHPLAN_DJIKSTRA_BENDYRULER = 3,
      74             :     };
      75             : 
      76             :     // enumeration for _OPTION parameter
      77             :     enum OARecoveryOptions {
      78             :         OA_OPTION_DISABLED = 0,
      79             :         OA_OPTION_WP_RESET = (1 << 0),
      80             :         OA_OPTION_LOG_DIJKSTRA_POINTS = (1 << 1),
      81             :         OA_OPTION_FAST_WAYPOINTS = (1 << 2),
      82             :     };
      83             : 
      84           0 :     uint16_t get_options() const { return _options;}
      85             : 
      86             :     static const struct AP_Param::GroupInfo var_info[];
      87             : 
      88             : private:
      89             : 
      90             :     // avoidance thread that continually updates the avoidance_result structure based on avoidance_request
      91             :     void avoidance_thread();
      92             :     bool start_thread();
      93             : 
      94             :     // helper function to map OABendyType to OAPathPlannerUsed
      95             :     OAPathPlannerUsed map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type);
      96             : 
      97             :     // an avoidance request from the navigation code
      98             :     struct avoidance_info {
      99             :         Location current_loc;
     100             :         Location origin;
     101             :         Location destination;
     102             :         Location next_destination;
     103             :         Vector2f ground_speed_vec;
     104             :         uint32_t request_time_ms;
     105             :     } avoidance_request, avoidance_request2;
     106             : 
     107             :     // an avoidance result from the avoidance thread
     108             :     struct {
     109             :         Location destination;       // destination vehicle is trying to get to (also used to verify the result matches a recent request)
     110             :         Location next_destination;  // next destination vehicle is trying to get to (also used to verify the result matches a recent request)
     111             :         Location origin_new;        // intermediate origin.  The start of line segment that vehicle should follow
     112             :         Location destination_new;   // intermediate destination vehicle should move towards
     113             :         Location next_destination_new; // intermediate next destination vehicle should move towards
     114             :         bool dest_to_next_dest_clear; // true if the path from destination_new to next_destination_new is clear and does not require path planning  (only supported by Dijkstras)
     115             :         uint32_t result_time_ms;    // system time the result was calculated (used to verify the result is recent)
     116             :         OAPathPlannerUsed path_planner_used;    // path planner that produced the result
     117             :         OA_RetState ret_state;      // OA_SUCCESS if the vehicle should move along the path from origin_new to destination_new
     118             :     } avoidance_result;
     119             : 
     120             :     // parameters
     121             :     AP_Int8 _type;                  // avoidance algorithm to be used
     122             :     AP_Float _margin_max;           // minimum distance in meters to keep from obstacles and fences
     123             :     AP_Int16 _options;              // Bitmask for options while recovering from Object Avoidance
     124             :     
     125             :     // internal variables used by front end
     126             :     HAL_Semaphore _rsem;            // semaphore for multi-thread use of avoidance_request and avoidance_result
     127             :     bool _thread_created;           // true once background thread has been created
     128             :     AP_OABendyRuler *_oabendyruler; // Bendy Ruler algorithm
     129             :     AP_OADijkstra *_oadijkstra;     // Dijkstra's algorithm
     130             :     AP_OADatabase _oadatabase;      // Database of dynamic objects to avoid
     131             :     uint32_t avoidance_latest_ms;   // last time Dijkstra's or BendyRuler algorithms ran (in the avoidance thread)
     132             :     uint32_t _last_update_ms;       // system time that mission_avoidance was called in main thread
     133             :     uint32_t _activated_ms;         // system time that object avoidance was most recently activated (used to avoid timeout error on first run)
     134             : 
     135             :     bool proximity_only = true;
     136             :     static AP_OAPathPlanner *_singleton;
     137             : };
     138             : 
     139             : namespace AP {
     140             :     AP_OAPathPlanner *ap_oapathplanner();
     141             : };
     142             : 
     143             : #endif  // AP_OAPATHPLANNER_ENABLED