diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 4416ed7187..ac45d68828 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/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; diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index a4648c6123..235234fd6a 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/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);