Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_GPS_SBF_ENABLED
       4             : 
       5             : #include "SIM_GPS_SBF.h"
       6             : 
       7             : #include <SITL/SITL.h>
       8             : #include <time.h>
       9             : 
      10             : using namespace SITL;
      11             : 
      12             : static const uint16_t CRC16_LOOK_UP[256] = {
      13             :     0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129,
      14             :     0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252,
      15             :     0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c,
      16             :     0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
      17             :     0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672,
      18             :     0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738,
      19             :     0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861,
      20             :     0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
      21             :     0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc,
      22             :     0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5,
      23             :     0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b,
      24             :     0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
      25             :     0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9,
      26             :     0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3,
      27             :     0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c,
      28             :     0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
      29             :     0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3,
      30             :     0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
      31             :     0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676,
      32             :     0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
      33             :     0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c,
      34             :     0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16,
      35             :     0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b,
      36             :     0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
      37             :     0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36,
      38             :     0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
      39             : };
      40             : 
      41         166 : void GPS_SBF::send_sbf(uint16_t msgid, uint8_t *buf, uint16_t size)
      42             : {
      43         166 :     if ((size % 4) != 0) {
      44           0 :         AP_HAL::panic("Invalid SBF packet length");
      45             :     }
      46             : 
      47             :     //HEADER
      48         166 :     const uint8_t PREAMBLE1 = 0x24;
      49         166 :     const uint8_t PREAMBLE2 = 0x40;
      50             :     uint8_t hdr[8];
      51         166 :     uint16_t crc = 0;
      52         166 :     hdr[0] = PREAMBLE1;
      53         166 :     hdr[1] = PREAMBLE2;
      54         166 :     hdr[4] = msgid & 0xFF;
      55         166 :     hdr[5] = (msgid >> 8) & 0xFF;
      56         166 :     hdr[6] = (size+8) & 0xFF;
      57         166 :     hdr[7] = ((size+8) >> 8) & 0xFF;
      58         830 :     for (uint8_t i = 4; i<8; i++) {
      59         664 :         crc = (crc << 8) ^ CRC16_LOOK_UP[uint8_t((crc >> 8) ^ hdr[i])]; //include id and length
      60             :     }
      61        9130 :     for (uint16_t i = 0; i<size; i++) {
      62        8964 :         crc = (crc << 8) ^ CRC16_LOOK_UP[uint8_t((crc >> 8) ^ buf[i])];
      63             :     }
      64         166 :     hdr[2] = crc & 0xFF;
      65         166 :     hdr[3] = (crc >> 8) & 0xFF;
      66             : 
      67         166 :     write_to_autopilot((char*)hdr, sizeof(hdr));
      68         166 :     write_to_autopilot((char*)buf, size);
      69         166 : }
      70             : 
      71          83 : void GPS_SBF::publish(const GPS_Data *d) {
      72          83 :     publish_PVTGeodetic(d);
      73          83 :     publish_DOP(d);
      74          83 : }
      75             : 
      76             : // public PVTGeodetic message, ID 4007
      77          83 : void GPS_SBF::publish_PVTGeodetic(const GPS_Data *d)
      78             : {
      79          83 :     const double DNU_DOUBLE = -2e10;
      80          83 :     const float DNU_FLOAT = -2e10;
      81          83 :     const uint8_t DNU_UINT8 = 255;
      82          83 :     const uint16_t DNU_UINT16 = 65535;
      83             : 
      84             :     struct PACKED timestamp_t {
      85             :         uint32_t tow;
      86             :         uint16_t wnc;
      87             :     };
      88             : 
      89             :     struct PACKED sbf_pvtGeod_t
      90             :     {
      91             :         timestamp_t time_stamp;
      92             :         uint8_t mode;
      93             :         uint8_t error;
      94             :         double latitude;
      95             :         double longitude;
      96             :         double height;
      97             :         float undulation;
      98             :         float vn;
      99             :         float ve;
     100             :         float vu;
     101             :         float cog;
     102             :         double rxclkbias;
     103             :         float rxclkdrift;
     104             :         uint8_t timesystem;
     105             :         uint8_t datum;
     106             :         uint8_t nrsv;
     107             :         uint8_t wacorrinfo;
     108             :         uint16_t referenceid;
     109             :         uint16_t meancorrage;
     110             :         uint64_t signalinfo;
     111             :         uint8_t alertflag;
     112             :         uint8_t __PADDING__[3];  // packets must be zero-mod-4
     113          83 :     } pvtGeod_buf {} ;
     114             :     assert_storage_size<sbf_pvtGeod_t, 84> assert_storage_size_pvt_Geod_buf;
     115             :     (void)assert_storage_size_pvt_Geod_buf;
     116             : 
     117          83 :     const uint16_t PVTGEO_0_MSG_ID = 0x0FA7;    
     118             : 
     119          83 :     const auto gps_tow = gps_time();
     120          83 :     pvtGeod_buf.time_stamp.tow = gps_tow.ms;
     121          83 :     pvtGeod_buf.time_stamp.wnc = gps_tow.week;
     122             :     
     123          83 :     pvtGeod_buf.mode = 4; //Mode: default to rtk fixed
     124          83 :     pvtGeod_buf.error= 0; //Error: no error
     125          83 :     pvtGeod_buf.latitude = radians(_sitl->state.latitude);
     126          83 :     pvtGeod_buf.longitude = radians(_sitl->state.longitude);
     127          83 :     pvtGeod_buf.height = d->altitude;
     128          83 :     pvtGeod_buf.undulation = DNU_DOUBLE;
     129          83 :     pvtGeod_buf.vn = d->speedN;
     130          83 :     pvtGeod_buf.ve = d->speedE;
     131          83 :     pvtGeod_buf.vu = -d->speedD;
     132          83 :     pvtGeod_buf.cog = degrees(d->ground_track_rad());
     133          83 :     pvtGeod_buf.rxclkbias = DNU_DOUBLE;
     134          83 :     pvtGeod_buf.rxclkdrift = DNU_FLOAT;
     135          83 :     pvtGeod_buf.timesystem = DNU_UINT8; 
     136          83 :     pvtGeod_buf.datum = DNU_UINT8; 
     137          83 :     pvtGeod_buf.nrsv = d->num_sats;
     138          83 :     pvtGeod_buf.wacorrinfo = 0; //default value
     139          83 :     pvtGeod_buf.referenceid = DNU_UINT16; 
     140          83 :     pvtGeod_buf.meancorrage = DNU_UINT16; 
     141          83 :     pvtGeod_buf.signalinfo = 0;
     142          83 :     pvtGeod_buf.alertflag = 0;
     143             : 
     144          83 :     send_sbf(PVTGEO_0_MSG_ID, (uint8_t*)&pvtGeod_buf, sizeof(pvtGeod_buf));
     145          83 : }
     146             : 
     147             : // public DOP message, ID 4001
     148          83 : void GPS_SBF::publish_DOP(const GPS_Data *d)
     149             : {
     150             :     struct PACKED timestamp_t {
     151             :         uint32_t tow;
     152             :         uint16_t wnc;
     153             :     };
     154             : 
     155          83 :     const auto gps_tow = gps_time();
     156             : 
     157             :     // swiped from the driver:
     158             :     const struct PACKED {
     159             :         timestamp_t time_stamp;
     160             :         uint8_t NrSV;
     161             :         uint8_t Reserved;
     162             :         uint16_t PDOP;
     163             :         uint16_t TDOP;
     164             :         uint16_t HDOP;
     165             :         uint16_t VDOP;
     166             :         float HPL;
     167             :         float VPL;
     168             :         // uint8_t __PADDING__[2];
     169          83 :     } packet {
     170          83 :         { gps_tow.ms, gps_tow.week },  // timestamp
     171             :         17,  // NrSV
     172             :         0,  // reserved
     173             :         1,  // PDOP
     174             :         1,  // TDOP
     175             :         1,  // HDOP
     176             :         1,  // VDOP
     177             :         1.0,  // HPL
     178             :         1.0  // VPL
     179          83 :     };
     180             : 
     181          83 :     send_sbf(4001, (uint8_t*)&packet, sizeof(packet));
     182          83 : }
     183             : 
     184             : #endif //AP_SIM_GPS_SBF_ENABLED