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             :     ADS-B RF based collision avoidance module
      19             :     https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
      20             : 
      21             :   Tom Pittenger, November 2015
      22             : */
      23             : 
      24             : #include "AP_ADSB_config.h"
      25             : 
      26             : #if HAL_ADSB_ENABLED
      27             : #include <AP_Common/AP_Common.h>
      28             : #include <AP_Param/AP_Param.h>
      29             : #include <AP_Common/Location.h>
      30             : #include <GCS_MAVLink/GCS_MAVLink.h>
      31             : #include <AP_GPS/AP_GPS_FixType.h>
      32             : 
      33             : #define ADSB_MAX_INSTANCES             1   // Maximum number of ADSB sensor instances available on this platform
      34             : 
      35             : #define ADSB_BITBASK_RF_CAPABILITIES_UAT_IN         (1 << 0)
      36             : #define ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN      (1 << 1)
      37             : #define ADSB_BITBASK_RF_CAPABILITIES_UAT_OUT        (1 << 2)
      38             : #define ADSB_BITBASK_RF_CAPABILITIES_1090ES_OUT     (1 << 3)
      39             : 
      40             : class AP_ADSB_Backend;
      41             : 
      42             : class AP_ADSB {
      43             : public:
      44             :     friend class AP_ADSB_Backend;
      45             :     friend class AP_ADSB_MAVLink;
      46             :     friend class AP_ADSB_uAvionix_UCP;
      47             :     friend class AP_ADSB_Sagetech;
      48             :     friend class AP_ADSB_Sagetech_MXS;
      49             : 
      50             :     // constructor
      51             :     AP_ADSB();
      52             : 
      53             :     /* Do not allow copies */
      54             :     CLASS_NO_COPY(AP_ADSB);
      55             : 
      56             :     // get singleton instance
      57       66592 :     static AP_ADSB *get_singleton(void) {
      58       66592 :         return _singleton;
      59             :     }
      60             : 
      61             :     // ADSB driver types
      62             :     enum class Type {
      63             :         None                = 0,
      64             :         MAVLink             = 1,
      65             :         Sagetech            = 2,
      66             :         uAvionix_UCP        = 3,
      67             :         Sagetech_MXS        = 4,
      68             :     };
      69             : 
      70             :     struct adsb_vehicle_t {
      71             :         mavlink_adsb_vehicle_t info; // the whole mavlink struct with all the juicy details. sizeof() == 38
      72             :         uint32_t last_update_ms; // last time this was refreshed, allows timeouts
      73             :     };
      74             : 
      75             :     // enum for adsb optional features
      76             :     enum class AdsbOption {
      77             :         Ping200X_Send_GPS               = (1<<0),
      78             :         Squawk_7400_FS_RC               = (1<<1),
      79             :         Squawk_7400_FS_GCS              = (1<<2),
      80             :         SagteTech_MXS_External_Config   = (1<<3),
      81             :         Mode3_Only                      = (1<<4),
      82             :     };
      83             : 
      84             :     // for holding parameters
      85             :     static const struct AP_Param::GroupInfo var_info[];
      86             : 
      87             :     // periodic task that maintains vehicle_list
      88             :     void update(void);
      89             : 
      90             :     // a structure holding *this vehicle's* position-related information:
      91             :     enum class AltType {
      92             :         Barometric = 0,  // we use a specific model for this?
      93             :         WGS84 = 1,
      94             :     };
      95             :     struct Loc : Location {
      96             :         AltType loc_alt_type;  // more information on altitude in base class
      97             : 
      98             :         AP_GPS_FixType fix_type;
      99             :         uint64_t epoch_us;  // microseconds since 1970-01-01
     100             :         uint64_t epoch_from_rtc_us;  // microseconds since 1970-01-01
     101             :         bool have_epoch_from_rtc_us;
     102             :         uint8_t satellites;
     103             : 
     104             :         float horizontal_pos_accuracy;
     105             :         bool horizontal_pos_accuracy_is_valid;
     106             : 
     107             :         float vertical_pos_accuracy;
     108             :         bool vertical_pos_accuracy_is_valid;
     109             : 
     110             :         float horizontal_vel_accuracy;
     111             :         bool horizontal_vel_accuracy_is_valid;
     112             : 
     113             :         Vector3f vel_ned;
     114             : 
     115             :         float vertRateD;  // m/s down
     116             :         bool vertRateD_is_valid;
     117             : 
     118             :         // methods to make us look much like the AP::gps() singleton:
     119           9 :         AP_GPS_FixType status() const { return fix_type; }
     120           0 :         const Vector3f &velocity() const {
     121           0 :             return vel_ned;
     122             :         }
     123           0 :         uint64_t time_epoch_usec() const { return epoch_us; }
     124             : 
     125             :         bool speed_accuracy(float &sacc) const;
     126             :         bool horizontal_accuracy(float &hacc) const;
     127             :         bool vertical_accuracy(float &vacc) const;
     128             : 
     129           0 :         uint8_t num_sats() const { return satellites; }
     130             : 
     131             :         // methods to make us look like the AP::ahrs() singleton:
     132          20 :         const Vector2f &groundspeed_vector() const { return vel_ned.xy(); }
     133          11 :         bool get_vert_pos_rate_D(float &velocity) const {
     134          11 :             velocity = vertRateD;
     135          11 :             return vertRateD_is_valid;
     136             :         }
     137             : 
     138             :         // data from a pressure sensor:
     139             :         bool baro_is_healthy;
     140             :         float baro_alt_press_diff_sea_level;
     141             : 
     142             :     } _my_loc;
     143             : 
     144             :     // periodic task that maintains vehicle_list
     145             :     void update(const Loc &loc);
     146             : 
     147             :     // send ADSB_VEHICLE mavlink message, usually as a StreamRate
     148             :     void send_adsb_vehicle(mavlink_channel_t chan);
     149             : 
     150       54357 :     bool set_stall_speed_cm(const uint16_t stall_speed_cm) {
     151       54357 :         if (out_state.cfg.was_set_externally) {
     152           0 :             return false;
     153             :         }
     154       54357 :         out_state.cfg.stall_speed_cm = stall_speed_cm;
     155       54357 :         return true;
     156             :     }
     157             : 
     158       54357 :     bool set_max_speed(int16_t max_speed) {
     159       54357 :         if (out_state.cfg.was_set_externally) {
     160           0 :             return false;
     161             :         }
     162             :         // convert m/s to knots
     163       54357 :         out_state.cfg.maxAircraftSpeed_knots = (float)max_speed * M_PER_SEC_TO_KNOTS;
     164       54357 :         return true;
     165             :     }
     166             : 
     167        3196 :     void set_is_auto_mode(const bool is_in_auto_mode) { out_state.is_in_auto_mode = is_in_auto_mode; }
     168      348648 :     void set_is_flying(const bool is_flying) { out_state.is_flying = is_flying; }
     169             : 
     170             :     // extract a location out of a vehicle item
     171             :     Location get_location(const adsb_vehicle_t &vehicle) const;
     172             : 
     173             :     // ADSB is considered enabled if there are any configured backends
     174       52280 :     bool enabled() const {
     175       52295 :         for (uint8_t instance=0; instance<detected_num_instances; instance++) {
     176        2938 :             if (_type[instance] > 0) {
     177        2923 :                 return true;
     178             :             }
     179             :         }
     180       49357 :         return false;
     181             :     }
     182             : 
     183         281 :     bool init_failed() const {
     184         281 :         return _init_failed;
     185             :     }
     186             : 
     187           2 :     bool healthy() {
     188           2 :         return check_startup();
     189             :     }
     190             : 
     191             :     bool next_sample(adsb_vehicle_t &obstacle);
     192             : 
     193             :     // handle a adsb_vehicle_t from an external source
     194             :     void handle_adsb_vehicle(const adsb_vehicle_t &vehicle);
     195             : 
     196             :     // mavlink message handler
     197             :     void handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg);
     198             : 
     199             :     void send_adsb_out_status(const mavlink_channel_t chan) const;
     200             : 
     201             :     // when true, a vehicle with that ICAO was found in database and the vehicle is populated.
     202             :     bool get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle) const;
     203             : 
     204             :     uint32_t get_special_ICAO_target() const { return (uint32_t)_special_ICAO_target; };
     205             :     void set_special_ICAO_target(const uint32_t new_icao_target) { _special_ICAO_target.set((int32_t)new_icao_target); };
     206       16783 :     bool is_special_vehicle(uint32_t icao) const { return _special_ICAO_target != 0 && (_special_ICAO_target == (int32_t)icao); }
     207             : 
     208             :     // confirm a value is a valid callsign
     209             :     static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED;
     210             : 
     211             :     static uint8_t convert_maxknots_to_enum(const float maxAircraftSpeed_knots);
     212             : 
     213             :     // Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value
     214             :     // stored on a GCS as a string field in different format, but then transmitted
     215             :     // over mavlink as a float which is always a decimal.
     216             :     static uint32_t convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber);
     217             : 
     218             :     // Trigger a Mode 3/A transponder IDENT. This should only be done when requested to do so by an Air Traffic Controller.
     219             :     // See wikipedia for IDENT explanation https://en.wikipedia.org/wiki/Transponder_(aeronautics)
     220             :     bool ident_start();
     221             : 
     222             :     AP_ADSB::Type get_type(uint8_t instance) const;
     223             : 
     224             : private:
     225             :     static AP_ADSB *_singleton;
     226             : 
     227             :     // initialize vehicle_list
     228             :     void init();
     229             : 
     230             :     // check to see if we are initialized (and possibly do initialization)
     231             :     bool check_startup();
     232             : 
     233             :     // compares current vector against vehicle_list to detect threats
     234             :     void determine_furthest_aircraft(void);
     235             : 
     236             :     // return index of given vehicle if ICAO_ADDRESS matches. return -1 if no match
     237             :     bool find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const;
     238             : 
     239             :     // remove a vehicle from the list
     240             :     void delete_vehicle(const uint16_t index);
     241             : 
     242             :     void set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle);
     243             : 
     244             :     // Generates pseudorandom ICAO from gps time, lat, and lon
     245             :     uint32_t genICAO(const Location &loc) const;
     246             : 
     247             :     // set callsign: 8char string (plus null termination) then optionally append last 4 digits of icao
     248             :     void set_callsign(const char* str, const bool append_icao);
     249             : 
     250             :     // configure ADSB-out transceivers
     251             :     void handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet);
     252             : 
     253             :     // control ADSB-out transcievers
     254             :     void handle_out_control(const mavlink_uavionix_adsb_out_control_t &packet);
     255             : 
     256             :     // mavlink handler
     257             :     void handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet);
     258             : 
     259             :     void detect_instance(uint8_t instance);
     260             : 
     261             :     AP_Int8 _type[ADSB_MAX_INSTANCES];
     262             : 
     263             :     bool _init_failed;
     264             : 
     265             :     // ADSB-IN state. Maintains list of external vehicles
     266             :     struct {
     267             :         // list management
     268             :         AP_Int16    list_size_param;
     269             :         uint16_t    list_size_allocated;
     270             :         adsb_vehicle_t *vehicle_list;
     271             :         uint16_t    vehicle_count;
     272             :         AP_Int32    list_radius;
     273             :         AP_Int16    list_altitude;
     274             : 
     275             :         // index of and distance to furthest vehicle in list
     276             :         uint16_t    furthest_vehicle_index;
     277             :         float       furthest_vehicle_distance;
     278             : 
     279             :         // streamrate stuff
     280             :         uint32_t    send_start_ms[MAVLINK_COMM_NUM_BUFFERS];
     281             :         uint16_t    send_index[MAVLINK_COMM_NUM_BUFFERS];
     282             :     } in_state;
     283             : 
     284             :     // ADSB-OUT state. Maintains export data
     285             :     struct {
     286             :         uint32_t    last_config_ms; // send once every 10s
     287             :         uint32_t    last_report_ms; // send at 5Hz
     288             :         int8_t      chan = -1; // channel that contains an ADS-b Transceiver. -1 means transceiver is not detected
     289             :         uint32_t    chan_last_ms;
     290             :         UAVIONIX_ADSB_RF_HEALTH status;     // transceiver status
     291             :         bool        is_flying;
     292             :         bool        is_in_auto_mode;
     293             : 
     294             :         // ADSB-OUT configuration
     295             :         struct {
     296             :             int32_t     ICAO_id;
     297             :             AP_Int32    ICAO_id_param;
     298             :             int32_t     ICAO_id_param_prev = -1; // assume we never send
     299             :             char        callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN]; //Vehicle identifier (8 characters, null terminated, valid characters are A-Z, 0-9, " " only).
     300             :             AP_Int8     emitterType;
     301             :             AP_Int8     lengthWidth;  // Aircraft length and width encoding (table 2-35 of DO-282B)
     302             :             AP_Int8     gpsOffsetLat;
     303             :             AP_Int8     gpsOffsetLon;
     304             :             uint16_t    stall_speed_cm;
     305             :             AP_Int8     rfSelect;
     306             :             AP_Int16    squawk_octal_param;
     307             :             uint16_t    squawk_octal;
     308             :             float       maxAircraftSpeed_knots;
     309             :             AP_Int8     rf_capable;
     310             :             bool        was_set_externally;
     311             :         } cfg;
     312             : 
     313             :         struct {
     314             :             bool                          baroCrossChecked;
     315             :             uint8_t                       airGroundState;
     316             :             bool                          identActive;
     317             :             bool                          modeAEnabled;
     318             :             bool                          modeCEnabled;
     319             :             bool                          modeSEnabled;
     320             :             bool                          es1090TxEnabled;
     321             :             int32_t                       externalBaroAltitude_mm;
     322             :             uint16_t                      squawkCode;
     323             :             uint8_t                       emergencyState;
     324             :             uint8_t                       callsign[8];
     325             :             bool                          x_bit;
     326             :         } ctrl;
     327             : 
     328             :         mavlink_uavionix_adsb_out_status_t tx_status;
     329             :     } out_state;
     330             : 
     331             :     uint8_t detected_num_instances;
     332             : 
     333             :     // special ICAO of interest that ignored filters when != 0
     334             :     AP_Int32 _special_ICAO_target;
     335             : 
     336             :     AP_Int32 _options;
     337             : 
     338             :     static const uint8_t _max_samples = 30;
     339             :     ObjectBuffer<adsb_vehicle_t> _samples{_max_samples};
     340             : 
     341             :     void push_sample(const adsb_vehicle_t &vehicle);
     342             : 
     343             :     // logging
     344             :     void write_log(const adsb_vehicle_t &vehicle) const;
     345             :     enum class Logging {
     346             :         NONE            = 0,
     347             :         SPECIAL_ONLY    = 1,
     348             :         ALL             = 2
     349             :     };
     350             :     AP_Enum<Logging> _log;
     351             : 
     352             :     // reference to backend
     353             :     AP_ADSB_Backend *_backend[ADSB_MAX_INSTANCES];
     354             : };
     355             : 
     356             : namespace AP {
     357             :     AP_ADSB *ADSB();
     358             : };
     359             : 
     360             : #endif // HAL_ADSB_ENABLED