Line data    Source code

       1             : #include "AP_DAL_GPS.h"
       2             : 
       3             : #include <AP_Logger/AP_Logger.h>
       4             : #include "AP_DAL.h"
       5             : 
       6       28509 : AP_DAL_GPS::AP_DAL_GPS()
       7             : {
       8        8772 :     for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
       9        6579 :         _RGPI[i].instance = i;
      10        6579 :         _RGPJ[i].instance = i;
      11             : #if AP_DAL_RGPK_LOGGING_ENABLED
      12             :         // _RGPK defaults to a zero offset so replay of older logs with no
      13             :         // RGPK messages applies no moving baseline yaw correction
      14        6579 :         _RGPK[i].instance = i;
      15             : #endif
      16             :     }
      17        2193 : }
      18             : 
      19    41156285 : void AP_DAL_GPS::start_frame()
      20             : {
      21    41156285 :     const auto &gps = AP::gps();
      22             : 
      23    41156285 :     const log_RGPH old_RGPH = _RGPH;
      24    41156285 :     _RGPH.primary_sensor = gps.primary_sensor();
      25    41156285 :     _RGPH.num_sensors = gps.num_sensors();
      26             : 
      27    41156285 :     WRITE_REPLAY_BLOCK_IFCHANGED(RGPH, _RGPH, old_RGPH);
      28             : 
      29   164625140 :     for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
      30   123468855 :         log_RGPI &RGPI = _RGPI[i];
      31   123468855 :         log_RGPJ &RGPJ = _RGPJ[i];
      32   123468855 :         const log_RGPI old_RGPI = RGPI;
      33   123468855 :         const log_RGPJ old_RGPJ = RGPJ;
      34             : 
      35   123468855 :         RGPI.status = (uint8_t)gps.status(i);
      36   123468855 :         RGPI.antenna_offset = gps.get_antenna_offset(i);
      37             : 
      38   123468855 :         const Location &loc = gps.location(i);
      39   123468855 :         RGPJ.last_message_time_ms = gps.last_message_time_ms(i);
      40   123468855 :         RGPJ.lat = loc.lat;
      41   123468855 :         RGPJ.lng = loc.lng;
      42   123468855 :         RGPJ.alt = loc.alt;
      43   123468855 :         RGPI.have_vertical_velocity = gps.have_vertical_velocity(i);
      44             : 
      45   123468855 :         RGPI.horizontal_accuracy_returncode = gps.horizontal_accuracy(i, RGPJ.hacc);
      46   123468855 :         RGPI.vertical_accuracy_returncode = gps.vertical_accuracy(i, RGPJ.vacc);
      47   123468855 :         RGPJ.hdop = gps.get_hdop(i);
      48   123468855 :         RGPI.num_sats = gps.num_sats(i);
      49   123468855 :         RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec);
      50             : 
      51   123468855 :         RGPJ.velocity = gps.velocity(i);
      52   123468855 :         RGPI.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc);
      53   123468855 :         RGPI.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg, RGPJ.yaw_deg_time_ms);
      54             : 
      55             : #if AP_DAL_RGPK_LOGGING_ENABLED
      56             :         // RGPK holds the body-frame antenna offset used to calculate gps_yaw.
      57             :         // The offset is derived from parameters but is zeroed while gps_yaw is
      58             :         // not calculated from a moving baseline, so it tracks the active yaw
      59             :         // source and yaw validity as well as parameter changes. All of these
      60             :         // change rarely, so the message is only written when changed
      61   123468855 :         log_RGPK &RGPK = _RGPK[i];
      62   123468855 :         const log_RGPK old_RGPK = RGPK;
      63   123468855 :         RGPK.mb_yaw_offset = gps.get_mb_yaw_offset(i);
      64             : #endif
      65             : 
      66   123468855 :         WRITE_REPLAY_BLOCK_IFCHANGED(RGPI, RGPI, old_RGPI);
      67   123468855 :         WRITE_REPLAY_BLOCK_IFCHANGED(RGPJ, RGPJ, old_RGPJ);
      68             : #if AP_DAL_RGPK_LOGGING_ENABLED
      69   123468855 :         WRITE_REPLAY_BLOCK_IFCHANGED(RGPK, RGPK, old_RGPK);
      70             : #endif
      71             : 
      72   123468855 :         tmp_location[i] = {
      73             :             RGPJ.lat,
      74             :             RGPJ.lng,
      75             :             RGPJ.alt,
      76             :             Location::AltFrame::ABSOLUTE
      77             :         };
      78             :     }
      79    41156285 : }