Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Param/AP_Param.h>
       4             : 
       5             : #define AP_NAKEKF_SOURCE_SET_MAX 3  // three sets of sources
       6             : 
       7             : class AP_NavEKF_Source
       8             : {
       9             : 
      10             : public:
      11             :     // Constructor
      12             :     AP_NavEKF_Source();
      13             : 
      14             :     /* Do not allow copies */
      15             :     CLASS_NO_COPY(AP_NavEKF_Source);
      16             : 
      17             :     enum class SourceXY : uint8_t {
      18             :         NONE = 0,
      19             :         // BARO = 1 (not applicable)
      20             :         // RANGEFINDER = 2 (not applicable)
      21             :         GPS = 3,
      22             :         BEACON = 4,
      23             :         OPTFLOW = 5,
      24             :         EXTNAV = 6,
      25             :         WHEEL_ENCODER = 7
      26             :     };
      27             : 
      28             :     enum class SourceZ : uint8_t {
      29             :         NONE = 0,
      30             :         BARO = 1,
      31             :         RANGEFINDER = 2,
      32             :         GPS = 3,
      33             :         BEACON = 4,
      34             :         // OPTFLOW = 5 (not applicable, optical flow can be used for terrain alt but not relative or absolute alt)
      35             :         EXTNAV = 6
      36             :         // WHEEL_ENCODER = 7 (not applicable)
      37             :     };
      38             : 
      39             :     enum class SourceYaw : uint8_t {
      40             :         NONE = 0,
      41             :         COMPASS = 1,
      42             :         GPS = 2,
      43             :         GPS_COMPASS_FALLBACK = 3,
      44             :         EXTNAV = 6,
      45             :         GSF = 8
      46             :     };
      47             : 
      48             :     // enum for OPTIONS parameter
      49             :     enum class SourceOptions {
      50             :         FUSE_ALL_VELOCITIES = (1 << 0),                 // fuse all velocities configured in source sets
      51             :         ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW = (1 << 1),  // align position of inactive sources to ahrs when using optical flow
      52             :         // reserved = (1 << 2),                          // reserved for future use
      53             :         SRC_PER_CORE = (1 << 3)                         // use a separate source set for each core
      54             :     };
      55             : 
      56             :     enum class SourceSetSelection : uint8_t {
      57             :         PRIMARY = 0,
      58             :         SECONDARY = 1,
      59             :         TERTIARY = 2,
      60             :     };
      61             : 
      62             :     // initialisation
      63             :     void init();
      64             : 
      65             :     // This function will get the active source set or get the source set for the core index if SRC_PER_CORE SourceOption is set
      66   626705447 :     uint8_t getActiveSourceSet(uint8_t core_index) const {
      67             :         // check if we are using a separate source set for each core
      68   626705447 :         if (option_is_set(SourceOptions::SRC_PER_CORE)) {
      69     1101549 :             if (core_index >= AP_NAKEKF_SOURCE_SET_MAX) {
      70             :                 // we need to return a valid source set
      71           0 :                 return active_source_set;
      72             :             }
      73     1101549 :             return core_index;
      74             :         }
      75   625603898 :         return active_source_set;
      76             :      }
      77             : 
      78             :     // get current position source
      79   242860188 :     SourceXY getPosXYSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].posxy; }
      80             : 
      81             :     SourceZ getPosZSource(uint8_t core_index) const;
      82             : 
      83             :     // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
      84             :     void setPosVelYawSourceSet(SourceSetSelection source_set_idx);
      85             : 
      86             :     // get/set velocity source
      87      963928 :     SourceXY getVelXYSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].velxy; }
      88      425032 :     SourceZ getVelZSource(uint8_t core_index) const { return _source_set[getActiveSourceSet(core_index)].velz; }
      89             : 
      90             :     // true/false of whether velocity source should be used
      91             :     bool useVelXYSource(SourceXY velxy_source, uint8_t core_index) const;
      92             :     bool useVelZSource(SourceZ velz_source, uint8_t core_index) const;
      93             : 
      94             :     // true if a velocity source is configured
      95             :     bool haveVelZSource(uint8_t core_index) const;
      96             : 
      97             :     // get yaw source
      98             :     SourceYaw getYawSource(uint8_t core_index) const;
      99             : 
     100             :     // align position of inactive sources to ahrs
     101             :     void align_inactive_sources();
     102             : 
     103             :     // sensor-specific helper functions
     104             : 
     105             :     // true if any source is GPS
     106             :     bool usingGPS(uint8_t core_index) const;
     107             : 
     108             :     // true if source parameters have been configured (used for parameter conversion)
     109             :     bool configured();
     110             : 
     111             :     // mark parameters as configured (used to ensure parameter conversion is only done once)
     112             :     void mark_configured();
     113             : 
     114             :     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
     115             :     // requires_position should be true if horizontal position configuration should be checked
     116             :     bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
     117             : 
     118             :     // return true if ext nav is enabled on any source
     119             :     bool ext_nav_enabled(void) const;
     120             : 
     121             :     // return true if GPS yaw is enabled on any source
     122             :     bool gps_yaw_enabled(void) const;
     123             : 
     124             :     // return true if wheel encoder is enabled on any source
     125             :     bool wheel_encoder_enabled(void) const;
     126             : 
     127             :     // returns active source set 
     128             :     uint8_t get_active_source_set() const;
     129             : 
     130             :     static const struct AP_Param::GroupInfo var_info[];
     131             : 
     132             : private:
     133             : 
     134             :     // Parameters
     135             :     struct SourceSet {
     136             :         AP_Enum<SourceXY>  posxy;  // xy position source
     137             :         AP_Enum<SourceXY>  velxy;  // xy velocity source
     138             :         AP_Enum<SourceZ>   posz;   // position z (aka altitude or height) source
     139             :         AP_Enum<SourceZ>   velz;   // velocity z source
     140             :         AP_Enum<SourceYaw> yaw;    // yaw source
     141             :     } _source_set[AP_NAKEKF_SOURCE_SET_MAX];
     142             : 
     143             :     // helper to check if an option parameter bit has been set
     144   716524485 :     bool option_is_set(SourceOptions option) const { return (_options.get() & int16_t(option)) != 0; }
     145             : 
     146             :     AP_Int16 _options;      // source options bitmask
     147             : 
     148             :     uint8_t active_source_set; // index of active source set
     149             :     bool _configured; // true once configured has returned true
     150             : };