SITL: Bump up read rate on SITL

* This is needed to do active configuration quickly
* Read/Write split and exposed to ensure physics/write rate is still
  coupled to avoid impacting the jamming and delayed data
* Created a utility to allocate the SITL instance

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

libraries/SITL/SIM_GPS.cpp

@@ -52,12 +52,6 @@ ssize_t GPS_Backend::read_from_autopilot(char *buffer, size_t size) const
     return front.read_from_autopilot(buffer, size);
 }
 
-void GPS_Backend::update(const GPS_Data &d)
-{
-    update_read(&d);
-    update_write(&d);
-}
-
 GPS::GPS(uint8_t _instance) :
     SerialDevice(8192, 2048),
     instance{_instance}
@@ -183,7 +177,7 @@ static GPS_TOW gps_time()
 /*
   send a new set of GPS UBLOX packets
  */
-void GPS_UBlox::update_write(const GPS_Data *d)
+void GPS_UBlox::publish(const GPS_Data *d)
 {
     struct PACKED ubx_nav_posllh {
         uint32_t    time; // GPS msToW
@@ -486,7 +480,7 @@ void GPS_NMEA::nmea_printf(const char *fmt, ...)
 /*
   send a new GPS NMEA packet
  */
-void GPS_NMEA::update_write(const GPS_Data *d)
+void GPS_NMEA::publish(const GPS_Data *d)
 {
     struct timeval tv;
     struct tm *tm;
@@ -599,7 +593,7 @@ void GPS_SBP_Common::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uin
     write_to_autopilot((char*)&crc, 2);
 }
 
-void GPS_SBP::update_write(const GPS_Data *d)
+void GPS_SBP::publish(const GPS_Data *d)
 {
     struct sbp_heartbeat_t {
         bool sys_error : 1;
@@ -713,7 +707,7 @@ void GPS_SBP::update_write(const GPS_Data *d)
 }
 
 
-void GPS_SBP2::update_write(const GPS_Data *d)
+void GPS_SBP2::publish(const GPS_Data *d)
 {
     struct sbp_heartbeat_t {
         bool sys_error : 1;
@@ -825,7 +819,7 @@ void GPS_SBP2::update_write(const GPS_Data *d)
     do_every_count++;
 }
 
-void GPS_NOVA::update_write(const GPS_Data *d)
+void GPS_NOVA::publish(const GPS_Data *d)
 {
     static struct PACKED nova_header
     {
@@ -991,7 +985,7 @@ uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_
     return( crc );
 }
 
-void GPS_GSOF::update_write(const GPS_Data *d)
+void GPS_GSOF::publish(const GPS_Data *d)
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
     constexpr uint8_t GSOF_POS_TIME_TYPE { 0x01 };
@@ -1286,7 +1280,7 @@ uint32_t GPS_GSOF::gsof_pack_float(const float& src)
 /*
   send MSP GPS data
  */
-void GPS_MSP::update_write(const GPS_Data *d)
+void GPS_MSP::publish(const GPS_Data *d)
 {
     struct PACKED {
         // header
@@ -1364,7 +1358,7 @@ void GPS_MSP::update_write(const GPS_Data *d)
   read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED
  */
 #if AP_SIM_GPS_FILE_ENABLED
-void GPS_FILE::update_write(const GPS_Data *d)
+void GPS_FILE::publish(const GPS_Data *d)
 {
     static int fd[2] = {-1,-1};
     static uint32_t base_time[2];
@@ -1524,6 +1518,7 @@ void GPS::update()
 
         // run at configured GPS rate (default 5Hz)
         if ((now_ms - last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) {
+            backend->update_read();
             return;
         }
 
@@ -1593,10 +1588,10 @@ void GPS::update()
         d.longitude += glitch_offsets.y;
         d.altitude += glitch_offsets.z;
 
-    backend->update(d);   // i.e. reading configuration etc from autopilot
+    backend->publish(&d);
 }
 
-void GPS_Backend::update_read(const GPS_Data *d)
+void GPS_Backend::update_read()
 {
         // swallow any config bytes
         char c;

libraries/SITL/SIM_GPS.h

@@ -69,14 +69,17 @@ public:
     GPS_Backend(class GPS &front, uint8_t _instance);
     virtual ~GPS_Backend() {}
 
-    void update(const GPS_Data &d);
-
     // 0 baud means "unset" i.e. baud-rate checks should not apply
     virtual uint32_t device_baud() const { return 0; }
 
     ssize_t write_to_autopilot(const char *p, size_t size) const;
     ssize_t read_from_autopilot(char *buffer, size_t size) const;
 
+    // read and process config from autopilot (e.g.)
+    virtual void update_read();
+    // writing fix information to autopilot (e.g.)
+    virtual void publish(const GPS_Data *d) = 0;
+    
 protected:
 
     uint8_t instance;
@@ -84,13 +87,6 @@ protected:
 
     class SIM *_sitl;
 
-private:
-
-    // read and process config from autopilot (e.g.)
-    virtual void update_read(const GPS_Data *d);
-    // writing fix information to autopilot (e.g.)
-    virtual void update_write(const GPS_Data *d) = 0;
-
 };
 
 class GPS_FILE : public GPS_Backend {
@@ -99,7 +95,7 @@ public:
 
     using GPS_Backend::GPS_Backend;
 
-    void update_write(const GPS_Data *d) override;
+    void publish(const GPS_Data *d) override;
 };
 
 class GPS_GSOF : public GPS_Backend {
@@ -108,7 +104,7 @@ public:
 
     using GPS_Backend::GPS_Backend;
 
-    void update_write(const GPS_Data *d) override;
+    void publish(const GPS_Data *d) override;
 
 private:
     void send_gsof(const uint8_t *buf, const uint16_t size);
@@ -125,7 +121,7 @@ public:
 
     using GPS_Backend::GPS_Backend;
 
-    void update_write(const GPS_Data *d) override;
+    void publish(const GPS_Data *d) override;
 
 private:
 
@@ -141,7 +137,7 @@ public:
 
     using GPS_Backend::GPS_Backend;
 
-    void update_write(const GPS_Data *d) override;
+    void publish(const GPS_Data *d) override;
 
     uint32_t device_baud() const override { return 19200; }
 
@@ -158,7 +154,7 @@ public:
 
     using GPS_Backend::GPS_Backend;
 
-    void update_write(const GPS_Data *d) override;
+    void publish(const GPS_Data *d) override;
 };
 
 class GPS_SBP_Common : public GPS_Backend {
@@ -179,7 +175,7 @@ public:
 
     using GPS_SBP_Common::GPS_SBP_Common;
 
-    void update_write(const GPS_Data *d) override;
+    void publish(const GPS_Data *d) override;
 
 };
 
@@ -189,7 +185,7 @@ public:
 
     using GPS_SBP_Common::GPS_SBP_Common;
 
-    void update_write(const GPS_Data *d) override;
+    void publish(const GPS_Data *d) override;
 
 };
 
@@ -199,7 +195,7 @@ public:
 
     using GPS_Backend::GPS_Backend;
 
-    void update_write(const GPS_Data *d) override;
+    void publish(const GPS_Data *d) override;
 
 private:
     void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);