Line data    Source code

       1             : #include "SIM_GPS_SBP2.h"
       2             : 
       3             : #if AP_SIM_GPS_SBP2_ENABLED
       4             : 
       5             : #include <SITL/SITL.h>
       6             : 
       7             : using namespace SITL;
       8             : 
       9          83 : void GPS_SBP2::publish(const GPS_Data *d)
      10             : {
      11             :     struct sbp_heartbeat_t {
      12             :         bool sys_error : 1;
      13             :         bool io_error : 1;
      14             :         bool nap_error : 1;
      15             :         uint8_t res : 5;
      16             :         uint8_t protocol_minor : 8;
      17             :         uint8_t protocol_major : 8;
      18             :         uint8_t res2 : 7;
      19             :         bool ext_antenna : 1;
      20             :     } hb; // 4 bytes
      21             : 
      22             :     struct PACKED sbp_gps_time_t {
      23             :         uint16_t wn;   //< GPS week number
      24             :         uint32_t tow;  //< GPS Time of Week rounded to the nearest ms
      25             :         int32_t ns;    //< Nanosecond remainder of rounded tow
      26             :         uint8_t flags; //< Status flags (reserved)
      27             :     } t;
      28             :     struct PACKED sbp_pos_llh_t {
      29             :         uint32_t tow;        //< GPS Time of Week
      30             :         double lat;          //< Latitude
      31             :         double lon;          //< Longitude
      32             :         double height;       //< Height
      33             :         uint16_t h_accuracy; //< Horizontal position accuracy estimate
      34             :         uint16_t v_accuracy; //< Vertical position accuracy estimate
      35             :         uint8_t n_sats;      //< Number of satellites used in solution
      36             :         uint8_t flags;       //< Status flags
      37             :     } pos;
      38             :     struct PACKED sbp_vel_ned_t {
      39             :         uint32_t tow;        //< GPS Time of Week
      40             :         int32_t n;           //< Velocity North coordinate
      41             :         int32_t e;           //< Velocity East coordinate
      42             :         int32_t d;           //< Velocity Down coordinate
      43             :         uint16_t h_accuracy; //< Horizontal velocity accuracy estimate
      44             :         uint16_t v_accuracy; //< Vertical velocity accuracy estimate
      45             :         uint8_t n_sats;      //< Number of satellites used in solution
      46             :         uint8_t flags;       //< Status flags (reserved)
      47             :     } velned;
      48             :     struct PACKED sbp_dops_t {
      49             :         uint32_t tow;  //< GPS Time of Week
      50             :         uint16_t gdop; //< Geometric Dilution of Precision
      51             :         uint16_t pdop; //< Position Dilution of Precision
      52             :         uint16_t tdop; //< Time Dilution of Precision
      53             :         uint16_t hdop; //< Horizontal Dilution of Precision
      54             :         uint16_t vdop; //< Vertical Dilution of Precision
      55             :         uint8_t flags; //< Status flags (reserved)
      56             :     } dops;
      57             : 
      58             :     static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF;
      59             :     static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0102;
      60             :     static const uint16_t SBP_DOPS_MSGTYPE = 0x0208;
      61             :     static const uint16_t SBP_POS_LLH_MSGTYPE = 0x020A;
      62             :     static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E;
      63             : 
      64          83 :     const auto gps_tow = gps_time();
      65             : 
      66          83 :     t.wn = gps_tow.week;
      67          83 :     t.tow = gps_tow.ms;
      68          83 :     t.ns = 0;
      69          83 :     t.flags = 1;
      70          83 :     sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
      71             : 
      72          83 :     if (!d->have_lock) {
      73           2 :         return;
      74             :     }
      75             : 
      76          81 :     pos.tow = gps_tow.ms;
      77          81 :     pos.lon = d->longitude;
      78          81 :     pos.lat= d->latitude;
      79          81 :     pos.height = d->altitude;
      80          81 :     pos.h_accuracy = d->horizontal_acc*1000;
      81          81 :     pos.v_accuracy = d->vertical_acc*1000;
      82          81 :     pos.n_sats = d->have_lock ? d->num_sats : 3;
      83             : 
      84             :     // Send single point position solution
      85          81 :     pos.flags = 1;
      86          81 :     sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
      87             :     // Send "pseudo-absolute" RTK position solution
      88          81 :     pos.flags = 4;
      89          81 :     sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
      90             : 
      91          81 :     velned.tow = gps_tow.ms;
      92          81 :     velned.n = 1e3 * d->speedN;
      93          81 :     velned.e = 1e3 * d->speedE;
      94          81 :     velned.d = 1e3 * d->speedD;
      95          81 :     velned.h_accuracy = 1e3 * 0.5;
      96          81 :     velned.v_accuracy = 1e3 * 0.5;
      97          81 :     velned.n_sats = d->have_lock ? d->num_sats : 3;
      98          81 :     velned.flags = 1;
      99          81 :     sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
     100             : 
     101             :     static uint32_t do_every_count = 0;
     102          81 :     if (do_every_count % 5 == 0) {
     103             : 
     104          17 :         dops.tow = gps_tow.ms;
     105          17 :         dops.gdop = 1;
     106          17 :         dops.pdop = 1;
     107          17 :         dops.tdop = 1;
     108          17 :         dops.hdop = 100;
     109          17 :         dops.vdop = 1;
     110          17 :         dops.flags = 1;
     111          17 :         sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
     112             :                           (uint8_t*)&dops);
     113             : 
     114          17 :         hb = {}; 
     115          17 :         hb.protocol_major = 2; //Sends protocol version 2.0
     116          17 :         sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb),
     117             :                           (uint8_t*)&hb);
     118             :     }
     119          81 :     do_every_count++;
     120             : }
     121             : 
     122             : #endif  // AP_SIM_GPS_SBP2_ENABLED