Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_GPS_NOVA_ENABLED
       4             : 
       5             : #include "SIM_GPS_NOVA.h"
       6             : 
       7             : #include <SITL/SITL.h>
       8             : 
       9             : using namespace SITL;
      10             : 
      11         135 : void GPS_NOVA::publish(const GPS_Data *d)
      12             : {
      13             :     static struct PACKED nova_header
      14             :     {
      15             :         // 0
      16             :         uint8_t preamble[3];
      17             :         // 3
      18             :         uint8_t headerlength;
      19             :         // 4
      20             :         uint16_t messageid;
      21             :         // 6
      22             :         uint8_t messagetype;
      23             :         //7
      24             :         uint8_t portaddr;
      25             :         //8
      26             :         uint16_t messagelength;
      27             :         //10
      28             :         uint16_t sequence;
      29             :         //12
      30             :         uint8_t idletime;
      31             :         //13
      32             :         uint8_t timestatus;
      33             :         //14
      34             :         uint16_t week;
      35             :         //16
      36             :         uint32_t tow;
      37             :         //20
      38             :         uint32_t recvstatus;
      39             :         // 24
      40             :         uint16_t resv;
      41             :         //26
      42             :         uint16_t recvswver;
      43             :     } header;
      44             : 
      45             :     struct PACKED psrdop
      46             :     {
      47             :         float gdop;
      48             :         float pdop;
      49             :         float hdop;
      50             :         float htdop;
      51             :         float tdop;
      52             :         float cutoff;
      53             :         uint32_t svcount;
      54             :         // extra data for individual prns
      55         135 :     } psrdop {};
      56             : 
      57             :     struct PACKED bestpos
      58             :     {
      59             :         uint32_t solstat;
      60             :         uint32_t postype;
      61             :         double lat;
      62             :         double lng;
      63             :         double hgt;
      64             :         float undulation;
      65             :         uint32_t datumid;
      66             :         float latsdev;
      67             :         float lngsdev;
      68             :         float hgtsdev;
      69             :         // 4 bytes
      70             :         uint8_t stnid[4];
      71             :         float diffage;
      72             :         float sol_age;
      73             :         uint8_t svstracked;
      74             :         uint8_t svsused;
      75             :         uint8_t svsl1;
      76             :         uint8_t svsmultfreq;
      77             :         uint8_t resv;
      78             :         uint8_t extsolstat;
      79             :         uint8_t galbeisigmask;
      80             :         uint8_t gpsglosigmask;
      81         135 :     } bestpos {};
      82             : 
      83             :     struct PACKED bestvel
      84             :     {
      85             :         uint32_t solstat;
      86             :         uint32_t veltype;
      87             :         float latency;
      88             :         float age;
      89             :         double horspd;
      90             :         double trkgnd;
      91             :         // + up
      92             :         double vertspd;
      93             :         float resv;
      94         135 :     } bestvel {};
      95             : 
      96         135 :     const auto gps_tow = gps_time();
      97             : 
      98         135 :     header.preamble[0] = 0xaa;
      99         135 :     header.preamble[1] = 0x44;
     100         135 :     header.preamble[2] = 0x12;
     101         135 :     header.headerlength = sizeof(header);
     102         135 :     header.week = gps_tow.week;
     103         135 :     header.tow = gps_tow.ms;
     104             : 
     105         135 :     header.messageid = 174;
     106         135 :     header.messagelength = sizeof(psrdop);
     107         135 :     header.sequence += 1;
     108             : 
     109         135 :     psrdop.hdop = 1.20;
     110         135 :     psrdop.htdop = 1.20;
     111         135 :     nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop));
     112             : 
     113             : 
     114         135 :     header.messageid = 99;
     115         135 :     header.messagelength = sizeof(bestvel);
     116         135 :     header.sequence += 1;
     117             : 
     118         135 :     bestvel.horspd = norm(d->speedN, d->speedE);
     119         135 :     bestvel.trkgnd = degrees(atan2f(d->speedE, d->speedN));
     120         135 :     bestvel.vertspd = -d->speedD;
     121             : 
     122         135 :     nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel));
     123             : 
     124             : 
     125         135 :     header.messageid = 42;
     126         135 :     header.messagelength = sizeof(bestpos);
     127         135 :     header.sequence += 1;
     128             : 
     129         135 :     bestpos.lat = d->latitude;
     130         135 :     bestpos.lng = d->longitude;
     131         135 :     bestpos.hgt = d->altitude;
     132         135 :     bestpos.svsused = d->have_lock ? d->num_sats : 3;
     133         135 :     bestpos.latsdev=0.2;
     134         135 :     bestpos.lngsdev=0.2;
     135         135 :     bestpos.hgtsdev=0.2;
     136         135 :     bestpos.solstat=0;
     137         135 :     bestpos.postype=32;
     138             : 
     139         135 :     nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos));
     140         135 : }
     141             : 
     142         405 : void GPS_NOVA::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen)
     143             : {
     144         405 :     write_to_autopilot((char*)header, headerlength);
     145         405 :     write_to_autopilot((char*)payload, payloadlen);
     146             : 
     147         405 :     uint32_t crc = crc_crc32((uint32_t)0, header, headerlength);
     148         405 :     crc = crc_crc32(crc, payload, payloadlen);
     149             : 
     150         405 :     write_to_autopilot((char*)&crc, 4);
     151         405 : }
     152             : 
     153             : #endif  // AP_SIM_GPS_NOVA_ENABLED