Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_GPS_NMEA_ENABLED
       4             : 
       5             : #include "SIM_GPS_NMEA.h"
       6             : 
       7             : #include <SITL/SITL.h>
       8             : #include <AP_Common/NMEA.h>
       9             : #include <AP_HAL/AP_HAL.h>
      10             : 
      11             : #include <time.h>
      12             : #include <sys/time.h>
      13             : 
      14             : extern const AP_HAL::HAL& hal;
      15             : 
      16             : using namespace SITL;
      17             : 
      18             : /*
      19             :   formatted print of NMEA message, with checksum appended
      20             :  */
      21        1538 : void GPS_NMEA::nmea_printf(const char *fmt, ...)
      22             : {
      23             :     va_list ap;
      24             : 
      25        1538 :     va_start(ap, fmt);
      26        1538 :     char *s = nmea_vaprintf(fmt, ap);
      27        1538 :     va_end(ap);
      28        1538 :     if (s != nullptr) {
      29        1538 :         write_to_autopilot((const char*)s, strlen(s));
      30        1538 :         free(s);
      31             :     }
      32        1538 : }
      33             : 
      34             : 
      35             : /*
      36             :   send a new GPS NMEA packet
      37             :  */
      38         406 : void GPS_NMEA::publish(const GPS_Data *d)
      39             : {
      40             :     struct timeval tv;
      41             :     struct tm *tm;
      42             :     char tstring[20];
      43             :     char dstring[20];
      44             :     char lat_string[20];
      45             :     char lng_string[20];
      46             : 
      47         406 :     simulation_timeval(&tv);
      48             : 
      49         406 :     struct tm tvd {};
      50         406 :     tm = gmtime_r(&tv.tv_sec, &tvd);
      51             : 
      52             :     // format time string
      53         406 :     hal.util->snprintf(tstring, sizeof(tstring), "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + tv.tv_usec*1.0e-6);
      54             : 
      55             :     // format date string
      56         406 :     hal.util->snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100);
      57             : 
      58             :     // format latitude
      59         406 :     double deg = fabs(d->latitude);
      60         406 :     hal.util->snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c",
      61             :              (unsigned)deg,
      62         406 :              (deg - int(deg))*60,
      63         406 :              d->latitude<0?'S':'N');
      64             : 
      65             :     // format longitude
      66         406 :     deg = fabs(d->longitude);
      67         406 :     hal.util->snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c",
      68             :              (unsigned)deg,
      69         406 :              (deg - int(deg))*60,
      70         406 :              d->longitude<0?'W':'E');
      71             : 
      72         412 :     nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
      73             :                      tstring,
      74             :                      lat_string,
      75             :                      lng_string,
      76         406 :                      d->have_lock?1:0,
      77         400 :                      d->have_lock?d->num_sats:3,
      78             :                      1.2,
      79         406 :                      d->altitude);
      80             : 
      81         406 :     const float speed_mps = d->speed_2d();
      82         406 :     const float speed_knots = speed_mps * M_PER_SEC_TO_KNOTS;
      83         406 :     const auto ground_track_deg = degrees(d->ground_track_rad());
      84             : 
      85             :     //$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24
      86         406 :     nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A",
      87             :                      tstring,
      88             :                      ground_track_deg,
      89             :                      ground_track_deg,
      90             :                      speed_knots,
      91         406 :                      speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6);
      92             : 
      93         406 :     nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
      94             :                      tstring,
      95         406 :                      d->have_lock?'A':'V',
      96             :                      lat_string,
      97             :                      lng_string,
      98             :                      speed_knots,
      99             :                      ground_track_deg,
     100             :                      dstring);
     101             : 
     102         406 :     if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_HDT) {
     103         320 :         nmea_printf("$GPHDT,%.2f,T", d->yaw_deg);
     104             :     }
     105          86 :     else if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_THS) {
     106           0 :         nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V');
     107          86 :     } else if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_KSXT) {
     108             :         // Unicore support
     109             :         // $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D
     110           0 :         nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,",
     111           0 :                     tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, unsigned(tv.tv_usec*1.e-4),
     112           0 :                     d->longitude, d->latitude,
     113           0 :                     d->altitude,
     114           0 :                     wrap_360(d->yaw_deg),
     115           0 :                     d->pitch_deg,
     116             :                     ground_track_deg,
     117             :                     speed_mps,
     118           0 :                     d->roll_deg,
     119           0 :                     d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed,
     120             :                     3, // fixed rtk yaw solution,
     121           0 :                     d->have_lock?d->num_sats:3,
     122           0 :                     d->have_lock?d->num_sats:3,
     123           0 :                     d->speedE * 3.6,
     124           0 :                     d->speedN * 3.6,
     125           0 :                     -d->speedD * 3.6);
     126             :     }
     127         406 : }
     128             : 
     129             : #endif  // AP_SIM_GPS_NMEA_ENABLED