Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AC_Avoidance_config.h"
       4             : 
       5             : #if AP_OADATABASE_ENABLED
       6             : 
       7             : #include <AP_HAL/Semaphores.h>
       8             : #include <AP_Math/AP_Math.h>
       9             : #include <GCS_MAVLink/GCS_MAVLink.h>
      10             : #include <AP_Param/AP_Param.h>
      11             : 
      12             : class AP_OADatabase {
      13             : public:
      14             : 
      15             :     AP_OADatabase();
      16             : 
      17             :     CLASS_NO_COPY(AP_OADatabase); /* Do not allow copies */
      18             : 
      19             :     // get singleton instance
      20      853612 :     static AP_OADatabase *get_singleton() {
      21      853612 :         return _singleton;
      22             :     }
      23             : 
      24             :     enum OA_DbItemImportance : uint8_t {
      25             :         Low,
      26             :         Normal,
      27             :         High,
      28             :     };
      29             : 
      30             :     struct OA_DbItem {
      31             :         Vector3f pos;           // position of the object as an offset in meters from the EKF origin
      32             :         uint32_t timestamp_ms;  // system time that object was last updated
      33             :         float radius;           // objects radius in meters
      34             : 
      35             :         // Unique ID, assigned by the source, not used by all sources
      36             :         uint32_t id;
      37             : 
      38             :         mavlink_channel_mask_t send_to_gcs;    // bitmask of mavlink comports to which details of this object should be sent
      39             :         OA_DbItemImportance importance;
      40             : 
      41             :         // Source of item, refresh treats sources differently
      42             :         enum class Source : uint8_t {
      43             :             proximity,
      44             :             AIS
      45             :         } source;
      46             :     };
      47             : 
      48             :     void init();
      49             :     void update();
      50             : 
      51             :     // Push an object into the database. Pos is the offset in meters from the EKF origin, measurement timestamp in ms, distance in meters, optional radius in meters
      52             :     void queue_push(const Vector3f &pos, const uint32_t timestamp_ms, const float distance, float radius, const OA_DbItem::Source source, const uint32_t id = 0);
      53             :     void queue_push(const Vector3f &pos, const uint32_t timestamp_ms, const float distance, const OA_DbItem::Source source, const uint32_t id = 0);
      54             : 
      55             :     // returns true if database is healthy
      56      893934 :     bool healthy() const { return (_queue.items != nullptr) && (_database.items != nullptr); }
      57             : 
      58             :     // fetch an item in database. Undefined result when i >= _database.count.
      59           0 :     const OA_DbItem& get_item(uint32_t i) const { return _database.items[i]; }
      60             : 
      61             :     // get number of items in the database
      62        6824 :     uint16_t database_count() const { return _database.count; }
      63             : 
      64             :     // empty queue and try and put into database. Return true if there's more work to do
      65             :     bool process_queue();
      66             : 
      67             :     // send ADSB_VEHICLE mavlink messages
      68             :     void send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_ms);
      69             : 
      70             :     static const struct AP_Param::GroupInfo var_info[];
      71             : 
      72             : private:
      73             : 
      74             :     // initialise
      75             :     void init_queue();
      76             :     void init_database();
      77             : 
      78             :     // database item management
      79             :     void database_item_add(const OA_DbItem &item);
      80             :     void database_item_refresh(OA_DbItem &current_item, const OA_DbItem &new_item) const;
      81             :     void database_item_remove(const uint16_t index);
      82             :     void database_items_remove_all_expired();
      83             : 
      84             :     // get bitmask of gcs channels item should be sent to based on its importance
      85             :     // returns 0xFF (send to all channels) if should be sent or 0 if it should not be sent
      86             :     uint8_t get_send_to_gcs_flags(const OA_DbItemImportance importance) const;
      87             : 
      88             :     // Return true if item A is likely the same as item B
      89             :     bool item_match(const OA_DbItem& A, const OA_DbItem& B) const;
      90             : 
      91             :     // enum for use with _OUTPUT parameter
      92             :     enum class OutputLevel {
      93             :         NONE = 0,
      94             :         HIGH = 1,
      95             :         HIGH_AND_NORMAL = 2,
      96             :         ALL = 3
      97             :     };
      98             : 
      99             :     // parameters
     100             :     AP_Int16        _queue_size_param;                      // queue size
     101             :     AP_Int16        _database_size_param;                   // db size
     102             :     AP_Int16        _database_expiry_seconds;               // objects expire after this timeout
     103             :     AP_Enum<OutputLevel> _output_level;                     // controls which items should be sent to GCS
     104             :     AP_Float        _beam_width;                            // beam width used when converting lidar readings to object radius
     105             :     AP_Float        _radius_min;                            // objects minimum radius (in meters)
     106             :     AP_Float        _dist_max;                              // objects maximum distance (in meters)
     107             :     AP_Float        _min_alt;                               // OADatabase minimum vehicle height check (in meters)
     108             : 
     109             :     struct {
     110             :         ObjectBuffer<OA_DbItem> *items;                     // thread safe incoming queue of points from proximity sensor to be put into database
     111             :         uint16_t        size;                               // cached value of _queue_size_param.
     112             :         HAL_Semaphore   sem;                                // semaphore for multi-thread use of queue
     113             :     } _queue;
     114             :     float dist_to_radius_scalar;                            // scalar to convert the distance and beam width to an object radius
     115             : 
     116             :     struct {
     117             :         OA_DbItem       *items;                             // array of objects in the database
     118             :         uint16_t        count;                              // number of objects in the items array
     119             :         uint16_t        size;                               // cached value of _database_size_param that sticks after initialized
     120             :     } _database;
     121             : 
     122             :     uint16_t _next_index_to_send[MAVLINK_COMM_NUM_BUFFERS]; // index of next object in _database to send to GCS
     123             :     uint16_t _highest_index_sent[MAVLINK_COMM_NUM_BUFFERS]; // highest index in _database sent to GCS
     124             :     uint32_t _last_send_to_gcs_ms[MAVLINK_COMM_NUM_BUFFERS];// system time that send_adsb_vehicle was last called
     125             : 
     126             :     static AP_OADatabase *_singleton;
     127             : };
     128             : 
     129             : namespace AP {
     130             :     AP_OADatabase *oadatabase();
     131             : };
     132             : 
     133             : #endif  // AP_OADATABASE_ENABLED