SITL: added simulated MSP GPS

tridge · Jul 31, 2023 · 7254e04 · 7254e04
1 parent 550f95f
commit 7254e04
Show file tree

Hide file tree

Showing 3 changed files with 86 additions and 2 deletions.
diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp
@@ -77,6 +77,7 @@ uint32_t GPS::device_baud() const
         case Type::NMEA:
         case Type::SBP:
         case Type::SBP2:
+        case Type::MSP:
 #if AP_SIM_GPS_FILE_ENABLED
         case Type::FILE:
 #endif
@@ -1016,6 +1017,83 @@ uint32_t GPS::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc
     return( crc );
 }
 
+/*
+  send MSP GPS data
+ */
+void GPS::update_msp(const struct gps_data *d)
+{
+    struct PACKED {
+        // header
+        struct PACKED {
+            uint8_t dollar = '$';
+            uint8_t magic = 'X';
+            uint8_t code = '<';
+            uint8_t  flags;
+            uint16_t cmd = 0x1F03; // GPS
+            uint16_t size = 52;
+        } hdr;
+        uint8_t  instance;
+        uint16_t gps_week;
+        uint32_t ms_tow;
+        uint8_t  fix_type;
+        uint8_t  satellites_in_view;
+        uint16_t horizontal_pos_accuracy;     // [cm]
+        uint16_t vertical_pos_accuracy;       // [cm]
+        uint16_t horizontal_vel_accuracy;     // [cm/s]
+        uint16_t hdop;
+        int32_t  longitude;
+        int32_t  latitude;
+        int32_t  msl_altitude;       // cm
+        int32_t  ned_vel_north;       // cm/s
+        int32_t  ned_vel_east;
+        int32_t  ned_vel_down;
+        uint16_t ground_course;      // deg * 100, 0..36000
+        uint16_t true_yaw;           // deg * 100, values of 0..36000 are valid. 65535 = no data available
+        uint16_t year;
+        uint8_t  month;
+        uint8_t  day;
+        uint8_t  hour;
+        uint8_t  min;
+        uint8_t  sec;
+
+        // footer CRC
+        uint8_t crc;
+    } msp_gps {};
+
+    auto t = gps_time();
+    struct timeval tv;
+    simulation_timeval(&tv);
+    auto *tm = gmtime(&tv.tv_sec);
+
+    msp_gps.gps_week = t.week;
+    msp_gps.ms_tow = t.ms;
+    msp_gps.fix_type = d->have_lock?3:0;
+    msp_gps.satellites_in_view = d->have_lock?_sitl->gps_numsats[instance]:3;
+    msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100;
+    msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100;
+    msp_gps.horizontal_vel_accuracy = 30;
+    msp_gps.hdop = 100;
+    msp_gps.longitude = d->longitude * 1.0e7;
+    msp_gps.latitude  = d->latitude * 1.0e7;
+    msp_gps.msl_altitude = d->altitude * 100;
+    msp_gps.ned_vel_north = 100 * d->speedN;
+    msp_gps.ned_vel_east = 100 * d->speedE;
+    msp_gps.ned_vel_down = 100 * d->speedD;
+    msp_gps.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100;
+    msp_gps.true_yaw = wrap_360(d->yaw_deg)*100U; // can send 65535 for no yaw
+    msp_gps.year = tm->tm_year;
+    msp_gps.month = tm->tm_mon;
+    msp_gps.day = tm->tm_mday;
+    msp_gps.hour = tm->tm_hour;
+    msp_gps.min = tm->tm_min;
+    msp_gps.sec = tm->tm_sec;
+
+    // CRC is over packet without first 3 bytes and trailing CRC byte
+    msp_gps.crc = crc8_dvb_s2_update(0, (uint8_t *)&msp_gps.hdr.flags, sizeof(msp_gps)-4);
+
+    write_to_autopilot((const char *)&msp_gps, sizeof(msp_gps));
+}
+
 /*
   read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED
  */
@@ -1218,6 +1296,10 @@ void GPS::update()
             update_nova(&d);
             break;
 
+        case Type::MSP:
+            update_msp(&d);
+            break;
+
 #if AP_SIM_GPS_FILE_ENABLED
         case Type::FILE:
             update_file();
@@ -1254,7 +1336,7 @@ GPS::gps_data GPS::interpolate_data(const gps_data &d, uint32_t delay_ms)
             d2.speedN += p * (s2.speedN - s1.speedN);
             d2.speedE += p * (s2.speedE - s1.speedE);
             d2.speedD += p * (s2.speedD - s1.speedD);
-            d2.yaw_deg += p * (s2.yaw_deg - s1.yaw_deg);
+            d2.yaw_deg += p * wrap_180(s2.yaw_deg - s1.yaw_deg);
             return d2;
         }
     }

diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h
@@ -59,6 +59,7 @@ class GPS : public SerialDevice {
 #endif
         NOVA  =  8,
         SBP2  =  9,
+        MSP  =  19,
     };
 
     GPS(uint8_t _instance);
@@ -115,6 +116,7 @@ class GPS : public SerialDevice {
 
     void update_sbp(const struct gps_data *d);
     void update_sbp2(const struct gps_data *d);
+    void update_msp(const struct gps_data *d);
 
 #if AP_SIM_GPS_FILE_ENABLED
     void update_file();

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
@@ -437,7 +437,7 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @Param: GPS_TYPE
     // @DisplayName: GPS 1 type
     // @Description: Sets the type of simulation used for GPS 1
-    // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP, 10:GSOF
+    // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP, 10:GSOF, 19:MSP
     // @User: Advanced
     AP_GROUPINFO("GPS_TYPE",       3, SIM,  gps_type[0],  GPS::Type::UBLOX),
     AP_GROUPINFO("GPS_BYTELOSS",   4, SIM,  gps_byteloss[0],  0),