Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_GPS_MSP_ENABLED
       4             : 
       5             : #include "SIM_GPS_MSP.h"
       6             : 
       7             : #include <SITL/SITL.h>
       8             : 
       9             : #include <time.h>
      10             : #include <sys/time.h>
      11             : 
      12             : using namespace SITL;
      13             : 
      14             : /*
      15             :   send MSP GPS data
      16             :  */
      17          84 : void GPS_MSP::publish(const GPS_Data *d)
      18             : {
      19             :     struct PACKED {
      20             :         // header
      21             :         struct PACKED {
      22             :             uint8_t dollar = '$';
      23             :             uint8_t magic = 'X';
      24             :             uint8_t code = '<';
      25             :             uint8_t  flags;
      26             :             uint16_t cmd = 0x1F03; // GPS
      27             :             uint16_t size = 52;
      28             :         } hdr;
      29             :         uint8_t  instance;
      30             :         uint16_t gps_week;
      31             :         uint32_t ms_tow;
      32             :         uint8_t  fix_type;
      33             :         uint8_t  satellites_in_view;
      34             :         uint16_t horizontal_pos_accuracy;     // [cm]
      35             :         uint16_t vertical_pos_accuracy;       // [cm]
      36             :         uint16_t horizontal_vel_accuracy;     // [cm/s]
      37             :         uint16_t hdop;
      38             :         int32_t  longitude;
      39             :         int32_t  latitude;
      40             :         int32_t  msl_altitude;       // cm
      41             :         int32_t  ned_vel_north;       // cm/s
      42             :         int32_t  ned_vel_east;
      43             :         int32_t  ned_vel_down;
      44             :         uint16_t ground_course;      // deg * 100, 0..36000
      45             :         uint16_t true_yaw;           // deg * 100, values of 0..36000 are valid. 65535 = no data available
      46             :         uint16_t year;
      47             :         uint8_t  month;
      48             :         uint8_t  day;
      49             :         uint8_t  hour;
      50             :         uint8_t  min;
      51             :         uint8_t  sec;
      52             : 
      53             :         // footer CRC
      54             :         uint8_t crc;
      55          84 :     } msp_gps {};
      56             : 
      57          84 :     auto t = gps_time();
      58             :     struct timeval tv;
      59          84 :     simulation_timeval(&tv);
      60          84 :     struct tm tvd {};
      61          84 :     auto *tm = gmtime_r(&tv.tv_sec, &tvd);
      62             : 
      63          84 :     msp_gps.gps_week = t.week;
      64          84 :     msp_gps.ms_tow = t.ms;
      65          84 :     msp_gps.fix_type = d->have_lock?3:0;
      66          84 :     msp_gps.satellites_in_view = d->have_lock ? d->num_sats : 3;
      67          84 :     msp_gps.horizontal_pos_accuracy = d->horizontal_acc*100;
      68          84 :     msp_gps.vertical_pos_accuracy = d->vertical_acc*100;
      69          84 :     msp_gps.horizontal_vel_accuracy = 30;
      70          84 :     msp_gps.hdop = 100;
      71          84 :     msp_gps.longitude = d->longitude * 1.0e7;
      72          84 :     msp_gps.latitude  = d->latitude * 1.0e7;
      73          84 :     msp_gps.msl_altitude = d->altitude * 100;
      74          84 :     msp_gps.ned_vel_north = 100 * d->speedN;
      75          84 :     msp_gps.ned_vel_east = 100 * d->speedE;
      76          84 :     msp_gps.ned_vel_down = 100 * d->speedD;
      77          84 :     msp_gps.ground_course = degrees(atan2f(d->speedE, d->speedN)) * 100;
      78          84 :     msp_gps.true_yaw = wrap_360(d->yaw_deg)*100U; // can send 65535 for no yaw
      79          84 :     msp_gps.year = tm->tm_year;
      80          84 :     msp_gps.month = tm->tm_mon;
      81          84 :     msp_gps.day = tm->tm_mday;
      82          84 :     msp_gps.hour = tm->tm_hour;
      83          84 :     msp_gps.min = tm->tm_min;
      84          84 :     msp_gps.sec = tm->tm_sec;
      85             : 
      86             :     // CRC is over packet without first 3 bytes and trailing CRC byte
      87          84 :     msp_gps.crc = crc8_dvb_s2_update(0, (uint8_t *)&msp_gps.hdr.flags, sizeof(msp_gps)-4);
      88             : 
      89          84 :     write_to_autopilot((const char *)&msp_gps, sizeof(msp_gps));
      90          84 : }
      91             : 
      92             : #endif  // AP_SIM_GPS_MSP_ENABLED