Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :    This program is free software: you can redistribute it and/or modify
       5             :    it under the terms of the GNU General Public License as published by
       6             :    the Free Software Foundation, either version 3 of the License, or
       7             :    (at your option) any later version.
       8             : 
       9             :    This program is distributed in the hope that it will be useful,
      10             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      12             :    GNU General Public License for more details.
      13             : 
      14             :    You should have received a copy of the GNU General Public License
      15             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : /*
      18             :   Situational awareness for ArduPilot
      19             : 
      20             :  - record a series of moving points in space which should be avoided
      21             :  - produce messages for GCS if a collision risk is detected
      22             : 
      23             :   Peter Barker, May 2016
      24             : 
      25             :   based on AP_ADSB,  Tom Pittenger, November 2015
      26             : */
      27             : 
      28             : #include "AP_Avoidance_config.h"
      29             : 
      30             : #if AP_ADSB_AVOIDANCE_ENABLED
      31             : 
      32             : #include <AP_ADSB/AP_ADSB.h>
      33             : 
      34             : #define AP_AVOIDANCE_STATE_RECOVERY_TIME_MS                 2000    // we will not downgrade state any faster than this (2 seconds)
      35             : 
      36             : #define AP_AVOIDANCE_ESCAPE_TIME_SEC                        2       // vehicle runs from thread for 2 seconds
      37             : 
      38             : class AP_Avoidance {
      39             : public:
      40             : 
      41             :     // constructor
      42             :     AP_Avoidance(class AP_ADSB &adsb);
      43             : 
      44             :     /* Do not allow copies */
      45             :     CLASS_NO_COPY(AP_Avoidance);
      46             : 
      47             :     // get singleton instance
      48           5 :     static AP_Avoidance *get_singleton() {
      49           5 :         return _singleton;
      50             :     }
      51             : 
      52             :     // F_RCVRY possible parameter values:
      53             :     enum class RecoveryAction {
      54             :         REMAIN_IN_AVOID_ADSB       = 0,
      55             :         RESUME_PREVIOUS_FLIGHTMODE = 1,
      56             :         RTL                        = 2,
      57             :         RESUME_IF_AUTO_ELSE_LOITER = 3,
      58             :     };
      59             : 
      60             :     // obstacle class to hold latest information for a known obstacles
      61             :     class Obstacle {
      62             :     public:
      63             :         MAV_COLLISION_SRC src;
      64             :         uint32_t src_id;
      65             :         uint32_t timestamp_ms;
      66             : 
      67             :         Location _location;
      68             :         Vector3f _velocity_ned_ms;
      69             : 
      70             :         // fields relating to this being a threat.  These would be the reason to have a separate list of threats:
      71             :         MAV_COLLISION_THREAT_LEVEL threat_level;
      72             :         float closest_approach_ne_m; // metres
      73             :         float closest_approach_d_m; // metres
      74             :         float time_to_closest_approach_s; // seconds, 3D approach
      75             :         float distance_to_closest_approach_ned_m; // metres, 3D
      76             :         uint32_t last_gcs_report_time; // millis
      77             :     };
      78             : 
      79             : 
      80             :     // add obstacle to the list of known obstacles
      81             :     void add_obstacle(uint32_t obstacle_timestamp_ms,
      82             :                       const MAV_COLLISION_SRC src,
      83             :                       uint32_t src_id,
      84             :                       const Location &loc,
      85             :                       const Vector3f &vel_ned_ms);
      86             : 
      87             :     void add_obstacle(uint32_t obstacle_timestamp_ms,
      88             :                       const MAV_COLLISION_SRC src,
      89             :                       uint32_t src_id,
      90             :                       const Location &loc,
      91             :                       float cog,
      92             :                       float hspeed,
      93             :                       float vspeed);
      94             : 
      95             :     // update should be called at 10hz or higher
      96             :     void update();
      97             : 
      98             :     // enable or disable avoidance
      99           2 :     void enable() { _enabled.set(true); };
     100           2 :     void disable() { _enabled.set(false); };
     101             : 
     102             :     // current overall threat level
     103             :     MAV_COLLISION_THREAT_LEVEL current_threat_level() const;
     104             : 
     105             :     // add obstacles into the Avoidance system from MAVLink messages
     106             :     void handle_msg(const mavlink_message_t &msg);
     107             : 
     108             :     // for holding parameters
     109             :     static const struct AP_Param::GroupInfo var_info[];
     110             : 
     111             : protected:
     112             : 
     113             :     // top level avoidance handler.  This calls the vehicle specific handle_avoidance with requested action
     114             :     void handle_avoidance_local(AP_Avoidance::Obstacle *threat);
     115             : 
     116             :     // avoid the most significant threat.  child classes must override this method
     117             :     // function returns the action that it is actually taking
     118             :     virtual MAV_COLLISION_ACTION handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) = 0;
     119             : 
     120             :     // recover after all threats have cleared.  child classes must override this method
     121             :     // recovery_action is from F_RCVRY parameter
     122             :     virtual void handle_recovery(RecoveryAction recovery_action) = 0;
     123             : 
     124             :     uint32_t _last_state_change_ms = 0;
     125             :     MAV_COLLISION_THREAT_LEVEL _threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
     126             : 
     127             :     // gcs notification
     128             :     // specifies how long we should continue sending messages about a threat after it has cleared
     129             :     static const uint8_t _gcs_cleared_messages_duration = 5; // seconds
     130             :     uint32_t _gcs_cleared_messages_first_sent;
     131             : 
     132             :     void handle_threat_gcs_notify(AP_Avoidance::Obstacle *threat);
     133             : 
     134             :     AP_Avoidance::Obstacle *most_serious_threat();
     135             : 
     136             :     // returns an entry from the MAV_COLLISION_ACTION representative
     137             :     // of what the current avoidance handler is up to.
     138          19 :     MAV_COLLISION_ACTION mav_avoidance_action() { return _latest_action; }
     139             : 
     140             :     // get target destination that best gets vehicle away from the nearest obstacle
     141             :     bool get_destination_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &newdest_neu, const float wp_speed_xy, const float wp_speed_z, const uint8_t _minimum_avoid_height);
     142             : 
     143             :     // get unit vector away from the nearest obstacle
     144             :     bool get_vector_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &vec_neu_unit) const;
     145             : 
     146             :     // helper functions to calculate destination to get us away from obstacle
     147             :     // Note: v1 is NED
     148             :     static Vector3f perpendicular_neu_m(const Location &p1, const Vector3f &v1, const Location &p2);
     149             : 
     150             : private:
     151             : 
     152             :     void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour) const;
     153             : 
     154             :     // constants
     155             :     static constexpr uint32_t MAX_OBSTACLE_AGE_MS = 5000;      // obstacles that have not been heard from for 5 seconds are removed from the list
     156             :     const static uint8_t _gcs_notify_interval = 1; // seconds
     157             : 
     158             :     // speed below which we will fly directly away from a threat
     159             :     // rather than perpendicular to its velocity:
     160             :     static constexpr uint8_t _low_velocity_threshold = 1; // meters/second
     161             : 
     162             :     // check to see if we are initialised (and possibly do initialisation)
     163             :     bool check_startup();
     164             : 
     165             :     // initialize _obstacle_list
     166             :     void init();
     167             : 
     168             :     // free _obstacle_list
     169             :     void deinit();
     170             : 
     171             :     // get unique id for adsb
     172             :     uint32_t src_id_for_adsb_vehicle(const AP_ADSB::adsb_vehicle_t &vehicle) const;
     173             : 
     174             :     void check_for_threats();
     175             :     void update_threat_level(const Location &my_loc,
     176             :                              const Vector3f &my_vel,
     177             :                              AP_Avoidance::Obstacle &obstacle);
     178             : 
     179             :     // calls into the AP_ADSB library to retrieve vehicle data
     180             :     void get_adsb_samples();
     181             : 
     182             :     // returns true if the obstacle should be considered more of a
     183             :     // threat than the current most serious threat
     184             :     bool obstacle_is_more_serious_threat(const AP_Avoidance::Obstacle &obstacle) const;
     185             : 
     186             :     // internal variables
     187             :     AP_Avoidance::Obstacle *_obstacles;
     188             :     uint8_t _obstacles_allocated;
     189             :     uint8_t _obstacle_count;
     190             :     int8_t _current_most_serious_threat;
     191             :     MAV_COLLISION_ACTION _latest_action = MAV_COLLISION_ACTION_NONE;
     192             : 
     193             :     // external references
     194             :     class AP_ADSB &_adsb;
     195             : 
     196             :     // parameters
     197             :     AP_Int8     _enabled;
     198             :     AP_Int8     _obstacles_max;
     199             : 
     200             :     AP_Int8     _fail_action;
     201             :     AP_Int8     _fail_recovery;
     202             :     AP_Int8     _fail_time_horizon_s;
     203             :     AP_Int16    _fail_distance_ne_m;
     204             :     AP_Int16    _fail_distance_d_m;
     205             :     AP_Int16    _fail_altitude_min_m;
     206             : 
     207             :     AP_Int8     _warn_action;
     208             :     AP_Int8     _warn_time_horizon_s;
     209             :     AP_Float    _warn_distance_ne_m;
     210             :     AP_Float    _warn_distance_d_m;
     211             : 
     212             :     // multi-thread support for avoidance
     213             :     HAL_Semaphore _rsem;
     214             : 
     215             :     static AP_Avoidance *_singleton;
     216             : };
     217             : 
     218             : float closest_approach_NE_m(const Location &my_loc,
     219             :                           const Vector3f &my_vel,
     220             :                           const Location &obstacle_loc,
     221             :                           const Vector3f &obstacle_vel,
     222             :                           uint8_t time_horizon);
     223             : 
     224             : float closest_approach_D_m(const Location &my_loc,
     225             :                          const Vector3f &my_vel,
     226             :                          const Location &obstacle_loc,
     227             :                          const Vector3f &obstacle_vel,
     228             :                          uint8_t time_horizon);
     229             : 
     230             : 
     231             : namespace AP {
     232             :     AP_Avoidance *ap_avoidance();
     233             : };
     234             : 
     235             : #endif  // AP_ADSB_AVOIDANCE_ENABLED