AP_HAL_SITL: make simulated GPS work as a SerialDevice

libraries/AP_HAL_SITL/HAL_SITL_Class.cpp

@@ -180,6 +180,11 @@ static void fill_stack_nan(void)
     fill_nanf(stk, ARRAY_SIZE(stk));
 }
 
+uint8_t HAL_SITL::get_instance() const
+{
+    return _sitl_state->get_instance();
+}
+
 void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
 {
     assert(callbacks);

libraries/AP_HAL_SITL/HAL_SITL_Class.h

@@ -35,6 +35,8 @@ public:
     }
     bool get_wipe_storage() const { return wipe_storage; }
 
+    uint8_t get_instance() const;
+
 private:
     HALSITL::SITL_State *_sitl_state;
 

libraries/AP_HAL_SITL/SITL_Periph_State.cpp

@@ -49,6 +49,7 @@ void SITL_State::init(int argc, char * const argv[]) {
                 exit(1);
         }
     }
+
     printf("Running Instance: %d\n", _instance);
 }
 
@@ -59,10 +60,6 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) {
 }
 
 
-int SITL_State::gps_pipe(uint8_t index) {
-    return 0;
-}
-
 int SITL_State::sim_fd(const char *name, const char *arg) {
     return 0;
 }

libraries/AP_HAL_SITL/SITL_Periph_State.h

@@ -29,7 +29,6 @@ class HALSITL::SITL_State {
     friend class HALSITL::GPIO;
 public:
     void init(int argc, char * const argv[]);
-    int gps_pipe(uint8_t index);
 
     // create a file descriptor attached to a virtual device; type of
     // device is given by name parameter

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -79,7 +79,6 @@ void SITL_State::_sitl_setup(const char *home_str)
     if (_sitl != nullptr) {
         // setup some initial values
         _update_airspeed(0);
-        _update_gps(0, 0, 0, 0, 0, 0, 0, false);
         _update_rangefinder(0);
         if (enable_gimbal) {
             gimbal = new SITL::Gimbal(_sitl->state);
@@ -164,17 +163,12 @@ void SITL_State::_fdm_input_step(void)
     _scheduler->sitl_begin_atomic();
 
     if (_update_count == 0 && _sitl != nullptr) {
-        _update_gps(0, 0, 0, 0, 0, 0, 0, false);
         HALSITL::Scheduler::timer_event();
         _scheduler->sitl_end_atomic();
         return;
     }
 
     if (_sitl != nullptr) {
-        _update_gps(_sitl->state.latitude, _sitl->state.longitude,
-                    _sitl->state.altitude,
-                    _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
-                    _sitl->state.yawDeg, true);
         _update_airspeed(_sitl->state.airspeed);
         _update_rangefinder(_sitl->state.range);
 
@@ -397,6 +391,17 @@ int SITL_State::sim_fd(const char *name, const char *arg)
         }
         ais = new SITL::AIS();
         return ais->fd();
+    } else if (strncmp(name, "gps", 3) == 0) {
+        const char *p = strchr(name, ':');
+        if (p == nullptr) {
+            AP_HAL::panic("Need a GPS number (e.g. sim:gps:1)");
+        }
+        uint8_t x = atoi(p+1);
+        if (x <= 0 || x > ARRAY_SIZE(gps)) {
+            AP_HAL::panic("Bad GPS number %u", x);
+        }
+        gps[x-1] = new SITL::GPS(x-1);
+        return gps[x-1]->fd();
     }
 
     AP_HAL::panic("unknown simulated device: %s", name);
@@ -534,6 +539,16 @@ int SITL_State::sim_fd_write(const char *name)
             AP_HAL::panic("No AIS created");
         }
         return ais->write_fd();
+    } else if (strncmp(name, "gps", 3) == 0) {
+        const char *p = strchr(name, ':');
+        if (p == nullptr) {
+            AP_HAL::panic("Need a GPS number (e.g. sim:gps:1)");
+        }
+        const uint8_t x = atoi(p+1);
+        if (x <= 0 || x > ARRAY_SIZE(gps)) {
+            AP_HAL::panic("Bad GPS number %u", x);
+        }
+        return gps[x-1]->write_fd();
     }
     AP_HAL::panic("unknown simulated device: %s", name);
 }
@@ -763,6 +778,11 @@ void SITL_State::_fdm_input_local(void)
     if (ais != nullptr) {
         ais->update();
     }
+    for (uint8_t i=0; i<ARRAY_SIZE(gps); i++) {
+        if (gps[i] != nullptr) {
+            gps[i]->update();
+        }
+    }
 
     if (_sitl && _use_fg_view) {
         _output_to_flightgear();

libraries/AP_HAL_SITL/SITL_State.h

@@ -46,6 +46,7 @@
 #include <SITL/SIM_VectorNav.h>
 #include <SITL/SIM_LORD.h>
 #include <SITL/SIM_AIS.h>
+#include <SITL/SIM_GPS.h>
 
 #include <SITL/SIM_Frsky_D.h>
 #include <SITL/SIM_CRSF.h>
@@ -76,7 +77,6 @@ public:
         Blimp
     };
 
-    int gps_pipe(uint8_t index);
     ssize_t gps_read(int fd, void *buf, size_t count);
     uint16_t pwm_output[SITL_NUM_CHANNELS];
     uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
@@ -145,42 +145,7 @@ private:
     void _update_rangefinder(float range_value);
     void _set_signal_handlers(void) const;
 
-    struct gps_data {
-        double latitude;
-        double longitude;
-        float altitude;
-        double speedN;
-        double speedE;
-        double speedD;
-        double yaw;
-        bool have_lock;
-    };
-
-#define MAX_GPS_DELAY 100
-    gps_data _gps_data[2][MAX_GPS_DELAY];
-
-    bool _gps_has_basestation_position;
-    gps_data _gps_basestation_data;
-    void _gps_write(const uint8_t *p, uint16_t size, uint8_t instance);
-    void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size, uint8_t instance);
-    void _update_gps_ubx(const struct gps_data *d, uint8_t instance);
-    uint8_t _gps_nmea_checksum(const char *s);
-    void _gps_nmea_printf(uint8_t instance, const char *fmt, ...);
-    void _update_gps_nmea(const struct gps_data *d, uint8_t instance);
-    void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload, uint8_t instance);
-    void _update_gps_sbp(const struct gps_data *d, uint8_t instance);
-    void _update_gps_sbp2(const struct gps_data *d, uint8_t instance);
-    void _update_gps_file(uint8_t instance);
-    void _update_gps_nova(const struct gps_data *d, uint8_t instance);
-    void _nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen, uint8_t instance);
-    uint32_t CRC32Value(uint32_t icrc);
-    uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
-
-    void _update_gps(double latitude, double longitude, float altitude,
-                     double speedN, double speedE, double speedD,
-                     double yaw, bool have_lock);
     void _update_airspeed(float airspeed);
-    void _update_gps_instance(SITL::SIM::GPSType gps_type, const struct gps_data *d, uint8_t instance);
     void _check_rc_input(void);
     bool _read_rc_sitl_input();
     void _fdm_input_local(void);
@@ -330,6 +295,9 @@ private:
 
     const char *_home_str;
     char *_gps_fifo[2];
+
+    // simulated GPS devices
+    SITL::GPS *gps[2];  // constrained by # of parameter sets
 };
 
 #endif // defined(HAL_BUILD_AP_PERIPH)

libraries/AP_HAL_SITL/UARTDriver.cpp

@@ -66,11 +66,13 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
     if (strcmp(path, "GPS1") == 0) {
         /* gps */
         _connected = true;
-        _fd = _sitlState->gps_pipe(0);
+        _fd = _sitlState->sim_fd("gps:1", "");
+        _fd_write = _sitlState->sim_fd_write("gps:1");
     } else if (strcmp(path, "GPS2") == 0) {
         /* 2nd gps */
         _connected = true;
-        _fd = _sitlState->gps_pipe(1);
+        _fd = _sitlState->sim_fd("gps:2", "");
+        _fd_write = _sitlState->sim_fd_write("gps:2");
     } else {
         /* parse type:args:flags string for path. 
            For example: