SITL: factor simulated GPS

libraries/SITL/SIM_GPS.cpp

@@ -44,6 +44,36 @@ struct GPS_TOW {
     uint32_t ms;
 };
 
+// ensure the backend we have allocated matches the one that's configured:
+GPS_Backend::GPS_Backend(GPS &_front, uint8_t _instance)
+    : front{_front},
+      instance{_instance}
+{
+}
+
+ssize_t GPS_Backend::write_to_autopilot(const char *p, size_t size) const
+{
+    return front.write_to_autopilot(p, size);
+}
+
+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)
+{
+    if (_sitl == nullptr) {
+        _sitl = AP::sitl();
+        if (_sitl == nullptr) {
+            return;
+        }
+    }
+
+    update_read(&d);
+    update_write(&d);
+}
+
 GPS::GPS(uint8_t _instance) :
     SerialDevice(8192, 2048),
     instance{_instance}
@@ -68,25 +98,10 @@ GPS::GPS(uint8_t _instance) :
 
 uint32_t GPS::device_baud() const
 {
-    if (_sitl == nullptr) {
+    if (backend == nullptr) {
         return 0;
     }
-    switch ((Type)_sitl->gps_type[instance].get()) {
+    return backend->device_baud();
-        case Type::NOVA:
-            return 19200;
-        case Type::NONE:
-        case Type::UBLOX:
-        case Type::NMEA:
-        case Type::SBP:
-        case Type::SBP2:
-        case Type::MSP:
-#if AP_SIM_GPS_FILE_ENABLED
-        case Type::FILE:
-#endif
-        case Type::GSOF:
-            return 0;  // 0 meaning unset
-    }
-    return 0;  // 0 meaning unset
 }
 
 /*
@@ -165,7 +180,7 @@ static void simulation_timeval(struct timeval *tv)
 /*
   send a UBLOX GPS message
  */
-void GPS::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
+void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
 {
     const uint8_t PREAMBLE1 = 0xb5;
     const uint8_t PREAMBLE2 = 0x62;
@@ -209,7 +224,7 @@ static GPS_TOW gps_time()
 /*
   send a new set of GPS UBLOX packets
  */
-void GPS::update_ubx(const struct gps_data *d)
+void GPS_UBlox::update_write(const GPS_Data *d)
 {
     struct PACKED ubx_nav_posllh {
         uint32_t    time; // GPS msToW
@@ -495,7 +510,7 @@ void GPS::update_ubx(const struct gps_data *d)
 /*
   formatted print of NMEA message, with checksum appended
  */
-void GPS::nmea_printf(const char *fmt, ...)
+void GPS_NMEA::nmea_printf(const char *fmt, ...)
 {
     va_list ap;
 
@@ -512,7 +527,7 @@ void GPS::nmea_printf(const char *fmt, ...)
 /*
   send a new GPS NMEA packet
  */
-void GPS::update_nmea(const struct gps_data *d)
+void GPS_NMEA::update_write(const GPS_Data *d)
 {
     struct timeval tv;
     struct tm *tm;
@@ -602,7 +617,7 @@ void GPS::update_nmea(const struct gps_data *d)
     }
 }
 
-void GPS::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload)
+void GPS_SBP_Common::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload)
 {
     if (len != 0 && payload == 0) {
         return; //SBP_NULL_ERROR;
@@ -625,7 +640,7 @@ void GPS::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, u
     write_to_autopilot((char*)&crc, 2);
 }
 
-void GPS::update_sbp(const struct gps_data *d)
+void GPS_SBP::update_write(const GPS_Data *d)
 {
     struct sbp_heartbeat_t {
         bool sys_error : 1;
@@ -739,7 +754,7 @@ void GPS::update_sbp(const struct gps_data *d)
 }
 
 
-void GPS::update_sbp2(const struct gps_data *d)
+void GPS_SBP2::update_write(const GPS_Data *d)
 {
     struct sbp_heartbeat_t {
         bool sys_error : 1;
@@ -851,7 +866,7 @@ void GPS::update_sbp2(const struct gps_data *d)
     do_every_count++;
 }
 
-void GPS::update_nova(const struct gps_data *d)
+void GPS_NOVA::update_write(const GPS_Data *d)
 {
     static struct PACKED nova_header
     {
@@ -982,7 +997,7 @@ void GPS::update_nova(const struct gps_data *d)
     nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos));
 }
 
-void GPS::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen)
+void GPS_NOVA::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen)
 {
     write_to_autopilot((char*)header, headerlength);
 write_to_autopilot((char*)payload, payloadlen);
@@ -994,7 +1009,7 @@ write_to_autopilot((char*)payload, payloadlen);
 }
 
 #define CRC32_POLYNOMIAL 0xEDB88320L
-uint32_t GPS::CRC32Value(uint32_t icrc)
+uint32_t GPS_NOVA::CRC32Value(uint32_t icrc)
 {
     int i;
     uint32_t crc = icrc;
@@ -1008,7 +1023,7 @@ uint32_t GPS::CRC32Value(uint32_t icrc)
     return crc;
 }
 
-uint32_t GPS::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc)
+uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc)
 {
     while ( length-- != 0 )
     {
@@ -1017,7 +1032,7 @@ uint32_t GPS::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc
     return( crc );
 }
 
-void GPS::update_gsof(const struct gps_data *d)
+void GPS_GSOF::update_write(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 };
@@ -1213,7 +1228,7 @@ void GPS::update_gsof(const struct gps_data *d)
 }
 
 
-void GPS::send_gsof(const uint8_t *buf, const uint16_t size)
+void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size)
 {
     // All Trimble "Data Collector" packets, including GSOF, are comprised of three fields:
     // * A fixed-length packet header (dcol_header)
@@ -1293,7 +1308,7 @@ void GPS::send_gsof(const uint8_t *buf, const uint16_t size)
     }
 }
 
-uint64_t GPS::pack_double_into_gsof_packet(const double& src)
+uint64_t GPS_GSOF::pack_double_into_gsof_packet(const double& src)
 {
     uint64_t dst;
     static_assert(sizeof(src) == sizeof(dst));
@@ -1302,7 +1317,7 @@ uint64_t GPS::pack_double_into_gsof_packet(const double& src)
     return dst;
 }
 
-uint32_t GPS::pack_float_into_gsof_packet(const float& src)
+uint32_t GPS_GSOF::pack_float_into_gsof_packet(const float& src)
 {
     uint32_t dst;
     static_assert(sizeof(src) == sizeof(dst));
@@ -1314,7 +1329,7 @@ uint32_t GPS::pack_float_into_gsof_packet(const float& src)
 /*
   send MSP GPS data
  */
-void GPS::update_msp(const struct gps_data *d)
+void GPS_MSP::update_write(const GPS_Data *d)
 {
     struct PACKED {
         // header
@@ -1392,7 +1407,7 @@ void GPS::update_msp(const struct gps_data *d)
   read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED
  */
 #if AP_SIM_GPS_FILE_ENABLED
-void GPS::update_file()
+void GPS_FILE::update_write(const GPS_Data *d)
 {
     static int fd[2] = {-1,-1};
     static uint32_t base_time[2];
@@ -1443,6 +1458,63 @@ rewind_file:
 }
 #endif  // AP_SIM_GPS_FILE_ENABLED
 
+void GPS::check_backend_allocation()
+{
+    const Type configured_type = Type(_sitl->gps_type[instance].get());
+    if (allocated_type == configured_type) {
+        return;
+    }
+
+    // mismatch; delete any already-allocated backend:
+    if (backend != nullptr) {
+        delete backend;
+        backend = nullptr;
+    }
+
+    // attempt to allocate backend
+    switch (configured_type) {
+    case Type::NONE:
+        // no GPS attached
+        break;
+
+    case Type::UBLOX:
+        backend = new GPS_UBlox(*this, instance);
+        break;
+
+    case Type::NMEA:
+        backend = new GPS_NMEA(*this, instance);
+        break;
+
+    case Type::SBP:
+        backend = new GPS_SBP(*this, instance);
+        break;
+
+    case Type::SBP2:
+        backend = new GPS_SBP2(*this, instance);
+        break;
+
+    case Type::NOVA:
+        backend = new GPS_NOVA(*this, instance);
+        break;
+
+    case Type::MSP:
+        backend = new GPS_MSP(*this, instance);
+        break;
+
+    case Type::GSOF:
+        backend = new GPS_GSOF(*this, instance);
+        break;
+
+#if AP_SIM_GPS_FILE_ENABLED
+    case Type::FILE:
+        backend = new GPS_FILE(*this, instance);
+        break;
+#endif
+    };
+
+    allocated_type = configured_type;
+}
+
 /*
   possibly send a new GPS packet
  */
@@ -1452,6 +1524,11 @@ void GPS::update()
         return;
     }
 
+    check_backend_allocation();
+    if (backend == nullptr) {
+        return;
+    }
+
     double latitude =_sitl->state.latitude;
     double longitude = _sitl->state.longitude;
     float altitude = _sitl->state.altitude;
@@ -1483,7 +1560,7 @@ void GPS::update()
 
     const uint8_t idx = instance;  // alias to avoid code churn
 
-        struct gps_data d {};
+        struct GPS_Data d {};
 
         // simulate delayed lock times
         bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
@@ -1493,11 +1570,7 @@ void GPS::update()
             return;
         }
 
-        // swallow any config bytes
+    last_update = now_ms;
-        char c;
-        read_from_autopilot(&c, 1);
-
-        last_update = now_ms;
 
         d.latitude = latitude;
         d.longitude = longitude;
@@ -1563,53 +1636,20 @@ void GPS::update()
         d.longitude += glitch_offsets.y;
         d.altitude += glitch_offsets.z;
 
+    backend->update(d);   // i.e. reading configuration etc from autopilot
+}
 
-    // do GPS-type-dependent updates:
+void GPS_Backend::update_read(const GPS_Data *d)
-    switch ((Type)_sitl->gps_type[instance].get()) {
+{
-        case Type::NONE:
+        // swallow any config bytes
-            // no GPS attached
+        char c;
-            break;
+        read_from_autopilot(&c, 1);
-
-        case Type::UBLOX:
-            update_ubx(&d);
-            break;
-
-        case Type::NMEA:
-            update_nmea(&d);
-            break;
-
-        case Type::SBP:
-            update_sbp(&d);
-            break;
-
-        case Type::SBP2:
-            update_sbp2(&d);
-            break;
-
-        case Type::NOVA:
-            update_nova(&d);
-            break;
-
-        case Type::MSP:
-            update_msp(&d);
-            break;
-
-        case Type::GSOF:
-            update_gsof(&d);
-            break;
-
-#if AP_SIM_GPS_FILE_ENABLED
-        case Type::FILE:
-            update_file();
-            break;
-#endif
-    }
 }
 
 /*
   get delayed data by interpolation
 */
-GPS::gps_data GPS::interpolate_data(const gps_data &d, uint32_t delay_ms)
+GPS_Data GPS::interpolate_data(const GPS_Data &d, uint32_t delay_ms)
 {
     const uint8_t N = ARRAY_SIZE(_gps_history);
     const uint32_t now_ms = d.timestamp_ms;
@@ -1624,9 +1664,9 @@ GPS::gps_data GPS::interpolate_data(const gps_data &d, uint32_t delay_ms)
         if (delay_ms >= dt1 && delay_ms <= dt2) {
             // we will interpolate this pair of samples. Start with
             // the older sample
-            const gps_data &s1 = _gps_history[i+1];
+            const GPS_Data &s1 = _gps_history[i+1];
-            const gps_data &s2 = _gps_history[i];
+            const GPS_Data &s2 = _gps_history[i];
-            gps_data d2 = s1;
+            GPS_Data d2 = s1;
             const float p = (dt2 - delay_ms) / MAX(1,float(dt2 - dt1));
             d2.latitude += p * (s2.latitude - s1.latitude);
             d2.longitude += p * (s2.longitude - s1.longitude);
@@ -1642,13 +1682,13 @@ GPS::gps_data GPS::interpolate_data(const gps_data &d, uint32_t delay_ms)
     return _gps_history[N-1];
 }
 
-float GPS::gps_data::heading() const
+float GPS_Data::heading() const
 {
     const auto velocity = Vector2d{speedE, speedN};
     return velocity.angle();
 }
 
-float GPS::gps_data::speed_2d() const
+float GPS_Data::speed_2d() const
 {
     const auto velocity = Vector2d{speedN, speedE};
     return velocity.length();

libraries/SITL/SIM_GPS.h

@@ -44,6 +44,169 @@ param set SERIAL5_PROTOCOL 5
 
 namespace SITL {
 
+// for delay simulation:
+struct GPS_Data {
+    uint32_t timestamp_ms;
+    double latitude;
+    double longitude;
+    float altitude;
+    double speedN;
+    double speedE;
+    double speedD;
+    double yaw_deg;
+    double roll_deg;
+    double pitch_deg;
+    bool have_lock;
+
+    // Get heading [rad], where 0 = North in WGS-84 coordinate system
+    float heading() const WARN_IF_UNUSED;
+
+    // Get 2D speed [m/s] in WGS-84 coordinate system
+    float speed_2d() const WARN_IF_UNUSED;
+};
+
+
+class GPS_Backend {
+public:
+    CLASS_NO_COPY(GPS_Backend);
+
+    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;
+
+protected:
+
+    uint8_t instance;
+    GPS &front;
+
+    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 {
+public:
+    CLASS_NO_COPY(GPS_FILE);
+
+    using GPS_Backend::GPS_Backend;
+
+    void update_write(const GPS_Data *d) override;
+};
+
+class GPS_GSOF : public GPS_Backend {
+public:
+    CLASS_NO_COPY(GPS_GSOF);
+
+    using GPS_Backend::GPS_Backend;
+
+    void update_write(const GPS_Data *d) override;
+
+private:
+    void send_gsof(const uint8_t *buf, const uint16_t size);
+
+    uint64_t pack_double_into_gsof_packet(const double& src) WARN_IF_UNUSED;
+    uint32_t pack_float_into_gsof_packet(const float& src) WARN_IF_UNUSED;
+};
+
+class GPS_NMEA : public GPS_Backend {
+public:
+    CLASS_NO_COPY(GPS_NMEA);
+
+    using GPS_Backend::GPS_Backend;
+
+    void update_write(const GPS_Data *d) override;
+
+private:
+
+    uint8_t nmea_checksum(const char *s);
+    void nmea_printf(const char *fmt, ...);
+    void update_nmea(const GPS_Data *d);
+
+};
+
+class GPS_NOVA : public GPS_Backend {
+public:
+    CLASS_NO_COPY(GPS_NOVA);
+
+    using GPS_Backend::GPS_Backend;
+
+    void update_write(const GPS_Data *d) override;
+
+    uint32_t device_baud() const override { return 19200; }
+
+private:
+
+    void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen);
+    uint32_t CRC32Value(uint32_t icrc);
+    uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
+};
+
+class GPS_MSP : public GPS_Backend {
+public:
+    CLASS_NO_COPY(GPS_MSP);
+
+    using GPS_Backend::GPS_Backend;
+
+    void update_write(const GPS_Data *d) override;
+};
+
+class GPS_SBP_Common : public GPS_Backend {
+public:
+    CLASS_NO_COPY(GPS_SBP_Common);
+
+    using GPS_Backend::GPS_Backend;
+
+protected:
+
+    void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
+
+};
+
+class GPS_SBP : public GPS_SBP_Common {
+public:
+    CLASS_NO_COPY(GPS_SBP);
+
+    using GPS_SBP_Common::GPS_SBP_Common;
+
+    void update_write(const GPS_Data *d) override;
+
+};
+
+class GPS_SBP2 : public GPS_SBP_Common {
+public:
+    CLASS_NO_COPY(GPS_SBP2);
+
+    using GPS_SBP_Common::GPS_SBP_Common;
+
+    void update_write(const GPS_Data *d) override;
+
+};
+
+class GPS_UBlox : public GPS_Backend {
+public:
+    CLASS_NO_COPY(GPS_UBlox);
+
+    using GPS_Backend::GPS_Backend;
+
+    void update_write(const GPS_Data *d) override;
+
+private:
+    void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
+};
+
 class GPS : public SerialDevice {
 public:
 
@@ -80,32 +243,8 @@ private:
 
     uint32_t last_update; // milliseconds
 
-    // for delay simulation:
-    struct gps_data {
-        uint32_t timestamp_ms;
-        double latitude;
-        double longitude;
-        float altitude;
-        double speedN;
-        double speedE;
-        double speedD;
-        double yaw_deg;
-        double roll_deg;
-        double pitch_deg;
-        bool have_lock;
-
-        // Get heading [rad], where 0 = North in WGS-84 coordinate system
-        float heading() const WARN_IF_UNUSED;
-
-        // Get 2D speed [m/s] in WGS-84 coordinate system
-        float speed_2d() const WARN_IF_UNUSED;
-    };
-
-
-
     // last 20 samples, allowing for up to 20 samples of delay
-    gps_data _gps_history[20];
+    GPS_Data _gps_history[20];
-
 
 #if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED
     // this will be allocated if needed:
@@ -113,40 +252,14 @@ private:
 #endif
 
     bool _gps_has_basestation_position;
-    gps_data _gps_basestation_data;
+    GPS_Data _gps_basestation_data;
-
-    void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
-    void update_ubx(const struct gps_data *d);
-
-    uint8_t nmea_checksum(const char *s);
-    void nmea_printf(const char *fmt, ...);
-    void update_nmea(const struct gps_data *d);
-
-    void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
-
-    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();
-#endif
-
-    void update_nova(const struct gps_data *d);
-    void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen);
-    uint32_t CRC32Value(uint32_t icrc);
-    uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
-
-    // If gsof gets data in, handle it. 
-    // Simply, it should respond to this: https://receiverhelp.trimble.com/oem-gnss/index.html#API_TestingComms.html
-    void on_data_gsof();
-    void update_gsof(const struct gps_data *d);
-    void send_gsof(const uint8_t *buf, const uint16_t size);
-    uint64_t pack_double_into_gsof_packet(const double& src) WARN_IF_UNUSED;
-    uint32_t pack_float_into_gsof_packet(const float& src) WARN_IF_UNUSED;
 
     // get delayed data
-    gps_data interpolate_data(const gps_data &d, uint32_t delay_ms);
+    GPS_Data interpolate_data(const GPS_Data &d, uint32_t delay_ms);
+
+    uint8_t allocated_type;
+    GPS_Backend *backend;
+    void check_backend_allocation();
 };
 
 }