Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_GPS/AP_GPS.h>
       4             : 
       5             : #include <AP_Logger/LogStructure.h>
       6             : 
       7             : class AP_DAL_GPS {
       8             : public:
       9             : 
      10             :     AP_DAL_GPS();
      11             : 
      12    37225724 :     AP_GPS_FixType status(uint8_t sensor_id) const {
      13    37225724 :         return (AP_GPS_FixType)_RGPI[sensor_id].status;
      14             :     }
      15      886487 :     AP_GPS_FixType status() const {
      16      886487 :         return status(primary_sensor());
      17             :     }
      18    15143241 :     const Location &location(uint8_t instance) const {
      19    15143241 :         return tmp_location[instance];
      20             :     }
      21     2224084 :     bool have_vertical_velocity(uint8_t instance) const {
      22     2224084 :         return _RGPI[instance].have_vertical_velocity;
      23             :     }
      24      877638 :     bool have_vertical_velocity() const {
      25      877638 :         return have_vertical_velocity(primary_sensor());
      26             :     }
      27     3315680 :     bool horizontal_accuracy(uint8_t instance, float &hacc) const {
      28     3315680 :         hacc = _RGPJ[instance].hacc;
      29     3315680 :         return _RGPI[instance].horizontal_accuracy_returncode;
      30             :     }
      31      622788 :     bool horizontal_accuracy(float &hacc) const {
      32      622788 :         return horizontal_accuracy(primary_sensor(),hacc);
      33             :     }
      34             : 
      35     4662126 :     bool vertical_accuracy(uint8_t instance, float &vacc) const {
      36     4662126 :         vacc = _RGPJ[instance].vacc;
      37     4662126 :         return _RGPI[instance].vertical_accuracy_returncode;
      38             :     }
      39      622788 :     bool vertical_accuracy(float &vacc) const {
      40      622788 :         return vertical_accuracy(primary_sensor(), vacc);
      41             :     }
      42             : 
      43     1657093 :     uint16_t get_hdop(uint8_t instance) const {
      44     1657093 :         return _RGPJ[instance].hdop;
      45             :     }
      46      310646 :     uint16_t get_hdop() const {
      47      310646 :         return get_hdop(primary_sensor());
      48             :     }
      49             : 
      50    24573347 :     uint32_t last_message_time_ms(uint8_t instance) const {
      51    24573347 :         return _RGPJ[instance].last_message_time_ms;
      52             :     }
      53             : 
      54     3316672 :     uint8_t num_sats(uint8_t instance) const {
      55     3316672 :         return _RGPI[instance].num_sats;
      56             :     }
      57      622788 :     uint8_t num_sats() const {
      58      622788 :         return num_sats(primary_sensor());
      59             :     }
      60             : 
      61     1706819 :     bool get_lag(uint8_t instance, float &lag_sec) const {
      62     1706819 :         lag_sec = _RGPI[instance].lag_sec;
      63     1706819 :         return _RGPI[instance].get_lag_returncode;
      64             :     }
      65      312142 :     bool get_lag(float &lag_sec) const {
      66      312142 :         return get_lag(primary_sensor(), lag_sec);
      67             :     }
      68             : 
      69     1658588 :     const Vector3f &velocity(uint8_t instance) const {
      70     1658588 :         return _RGPJ[instance].velocity;
      71             :     }
      72      312142 :     const Vector3f &velocity() const {
      73      312142 :         return velocity(primary_sensor());
      74             :     }
      75             : 
      76     3317176 :     bool speed_accuracy(uint8_t instance, float &sacc) const {
      77     3317176 :         sacc = _RGPJ[instance].sacc;
      78     3317176 :         return _RGPI[instance].speed_accuracy_returncode;
      79             :     }
      80      624284 :     bool speed_accuracy(float &sacc) const {
      81      624284 :         return speed_accuracy(primary_sensor(), sacc);
      82             :     }
      83             : 
      84    17816285 :     bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const {
      85    17816285 :         yaw_deg = _RGPJ[instance].yaw_deg;
      86    17816285 :         accuracy_deg = _RGPJ[instance].yaw_accuracy_deg;
      87    17816285 :         time_ms = _RGPJ[instance].yaw_deg_time_ms;
      88    17816285 :         return _RGPI[instance].gps_yaw_deg_returncode;
      89             :     }
      90             : 
      91      248527 :     uint8_t num_sensors(void) const {
      92      248527 :         return _RGPH.num_sensors;
      93             :     }
      94             : 
      95    89440770 :     uint8_t primary_sensor(void) const {
      96    89440770 :         return _RGPH.primary_sensor;
      97             :     }
      98             : 
      99             :     // TODO: decide if this really, really should be here!
     100     1431965 :     const Location &location() const {
     101     1431965 :         return location(_RGPH.primary_sensor);
     102             :     }
     103             : 
     104             :     // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
     105     1507124 :     const Vector3f &get_antenna_offset(uint8_t instance) const {
     106     1507124 :         return _RGPI[instance].antenna_offset;
     107             :     }
     108             : 
     109             : #if AP_DAL_RGPK_LOGGING_ENABLED
     110             :     // return the body-frame moving baseline antenna offset used to calculate
     111             :     // the yaw returned by gps_yaw_deg for this instance, zero when that yaw
     112             :     // is not derived from a moving baseline
     113       10752 :     const Vector3f &get_mb_yaw_offset(uint8_t instance) const {
     114       10752 :         return _RGPK[instance].mb_yaw_offset;
     115             :     }
     116             : #endif
     117             : 
     118             :     void start_frame();
     119             : 
     120             :     void handle_message(const log_RGPH &msg) {
     121             :         _RGPH = msg;
     122             :     }
     123             :     void handle_message(const log_RGPI &msg) {
     124             :         _RGPI[msg.instance] = msg;
     125             :     }
     126             :     void handle_message(const log_RGPJ &msg) {
     127             :         _RGPJ[msg.instance] = msg;
     128             : 
     129             :         tmp_location[msg.instance].lat = msg.lat;
     130             :         tmp_location[msg.instance].lng = msg.lng;
     131             :         tmp_location[msg.instance].alt = msg.alt;
     132             :     }
     133             : #if AP_DAL_RGPK_LOGGING_ENABLED
     134             :     void handle_message(const log_RGPK &msg) {
     135             :         if (msg.instance < ARRAY_SIZE(_RGPK)) {
     136             :             _RGPK[msg.instance] = msg;
     137             :         }
     138             :     }
     139             : #endif
     140             : 
     141             : private:
     142             : 
     143             :     struct log_RGPH _RGPH;
     144             :     struct log_RGPI _RGPI[GPS_MAX_INSTANCES];
     145             :     struct log_RGPJ _RGPJ[GPS_MAX_INSTANCES];
     146             : #if AP_DAL_RGPK_LOGGING_ENABLED
     147             :     struct log_RGPK _RGPK[GPS_MAX_INSTANCES];
     148             : #endif
     149             : 
     150             :     Location tmp_location[GPS_MAX_INSTANCES];
     151             : };