add ability to simulate more than 2 GPSs

Author: peterbarker

libraries/AP_HAL_SITL/SITL_State_common.cpp

@@ -304,13 +304,9 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
         return ais;
 #endif
     } 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);
+        uint8_t x = atoi(arg);
         if (x <= 0 || x > ARRAY_SIZE(gps)) {
-            AP_HAL::panic("Bad GPS number %u", x);
+            AP_HAL::panic("Bad GPS number %u (%s)", x, arg);
         }
         gps[x-1] = NEW_NOTHROW SITL::GPS(x-1);
         return gps[x-1];

libraries/AP_HAL_SITL/SITL_State_common.h

@@ -231,7 +231,7 @@ class HALSITL::SITL_State_Common {
     const char *defaults_path = HAL_PARAM_DEFAULTS_PATH;
 
     // simulated GPS devices
-    SITL::GPS *gps[2];  // constrained by # of parameter sets
+    SITL::GPS *gps[AP_SIM_MAX_GPS_SENSORS];  // constrained by # of parameter sets
 
     // Simulated ELRS radio
     SITL::ELRS *elrs;

libraries/AP_HAL_SITL/UARTDriver.cpp

@@ -77,11 +77,11 @@ void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
     if (strcmp(path, "GPS1") == 0) {
         /* gps */
         _connected = true;
-        _sim_serial_device = _sitlState->create_serial_sim("gps:1", "", _portNumber);
+        _sim_serial_device = _sitlState->create_serial_sim("gps", "1", _portNumber);
     } else if (strcmp(path, "GPS2") == 0) {
         /* 2nd gps */
         _connected = true;
-        _sim_serial_device = _sitlState->create_serial_sim("gps:2", "", _portNumber);
+        _sim_serial_device = _sitlState->create_serial_sim("gps", "2", _portNumber);
     } else {
         /* parse type:args:flags string for path. 
            For example:

libraries/SITL/SIM_GPS.cpp

@@ -30,6 +30,105 @@
 
 #include <GCS_MAVLink/GCS.h>
 
+namespace SITL {
+// user settable parameters for airspeed sensors
+const AP_Param::GroupInfo SIM::GPSParms::var_info[] = {
+    // @Param: DISABLE
+    // @DisplayName: GPS disable
+    // @Description: Disables GPS
+    // @Values: 0:Enable, 1:GPS Disabled
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("ENABLE",    1, GPSParms, enabled, 0, AP_PARAM_FLAG_ENABLE),
+    // @Param: LAG_MS
+    // @DisplayName: GPS Lag
+    // @Description: GPS lag
+    // @Units: ms
+    // @User: Advanced
+    AP_GROUPINFO("LAG_MS",     2, GPSParms, delay_ms, 100),
+    // @Param: TYPE
+    // @DisplayName: GPS type
+    // @Description: Sets the type of simulation used for GPS
+    // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP2, 11:Trimble, 19:MSP
+    // @User: Advanced
+    AP_GROUPINFO("TYPE",       3, GPSParms, type,  GPS::Type::UBLOX),
+    // @Param: BYTELOS
+    // @DisplayName: GPS Byteloss
+    // @Description: Percent of bytes lost from GPS
+    // @Units: %
+    // @User: Advanced
+    AP_GROUPINFO("BYTELOS",   4, GPSParms, byteloss,  0),
+    // @Param: NUMSATS
+    // @DisplayName: GPS Num Satellites
+    // @Description: Number of satellites GPS has in view
+    AP_GROUPINFO("NUMSATS",    5, GPSParms, numsats,   10),
+    // @Param: GLITCH
+    // @DisplayName: GPS Glitch
+    // @Description: Glitch offsets of simulated GPS sensor
+    // @Vector3Parameter: 1
+    // @User: Advanced
+    AP_GROUPINFO("GLITCH",     6, GPSParms, glitch,  0),
+    // @Param: HZ
+    // @DisplayName: GPS Hz
+    // @Description: GPS Update rate
+    // @Units: Hz
+    AP_GROUPINFO("HZ",         7, GPSParms, hertz,  5),
+    // @Param: DRFTALT
+    // @DisplayName: GPS Altitude Drift
+    // @Description: GPS altitude drift error
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("DRFTALT",   8, GPSParms, drift_alt, 0),
+    // @Param: POS
+    // @DisplayName: GPS Position
+    // @Description: GPS antenna phase center position relative to the body frame origin
+    // @Units: m
+    // @Vector3Parameter: 1
+    AP_GROUPINFO("POS",        9, GPSParms, pos_offset, 0),
+    // @Param: NOISE
+    // @DisplayName: GPS Noise
+    // @Description: Amplitude of the GPS1 altitude error
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("NOISE",     10, GPSParms, noise, 0),
+    // @Param: LCKTIME
+    // @DisplayName: GPS Lock Time
+    // @Description: Delay in seconds before GPS1 acquires lock
+    // @Units: s
+    // @User: Advanced
+    AP_GROUPINFO("LCKTIME",  11, GPSParms, lock_time, 0),
+    // @Param: ALT_OFS
+    // @DisplayName: GPS Altitude Offset
+    // @Description: GPS Altitude Error
+    // @Units: m
+    AP_GROUPINFO("ALT_OFS",   12, GPSParms, alt_offset, 0),
+    // @Param: HDG
+    // @DisplayName: GPS Heading
+    // @Description: Enable GPS1 output of NMEA heading HDT sentence or UBLOX_RELPOSNED
+    // @Values: 0:Disabled, 1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("HDG",       13, GPSParms, hdg_enabled, SIM::GPS_HEADING_NONE),
+    // @Param: ACC
+    // @DisplayName: GPS Accuracy
+    // @Description: GPS Accuracy
+    // @User: Advanced
+    AP_GROUPINFO("ACC",       14, GPSParms, accuracy, 0.3),
+    // @Param: VERR
+    // @DisplayName: GPS Velocity Error
+    // @Description: GPS Velocity Error Offsets in NED
+    // @Vector3Parameter: 1
+    // @User: Advanced
+    AP_GROUPINFO("VERR",      15, GPSParms, vel_err, 0),
+    // @Param: JAM
+    // @DisplayName: GPS jamming enable
+    // @Description: Enable simulated GPS jamming
+    // @User: Advanced
+    // @Values: 0:Disabled, 1:Enabled
+    AP_GROUPINFO("JAM",       16, GPSParms, jam, 0),
+
+    AP_GROUPEND
+};
+}
+
 // the number of GPS leap seconds - copied from AP_GPS.h
 #define GPS_LEAPSECONDS_MILLIS 18000ULL
 
@@ -43,6 +142,13 @@ GPS_Backend::GPS_Backend(GPS &_front, uint8_t _instance) :
     front{_front}
 {
     _sitl = AP::sitl();
+
+#if HAL_SIM_GPS_ENABLED && AP_SIM_MAX_GPS_SENSORS > 0
+    // default the first backend to enabled:
+    if (_instance == 0 && !_sitl->gps[0].enabled.configured()) {
+        _sitl->gps[0].enabled.set(1);
+    }
+#endif
 }
 
 ssize_t GPS_Backend::write_to_autopilot(const char *p, size_t size) const
@@ -78,11 +184,11 @@ ssize_t GPS::write_to_autopilot(const char *p, size_t size) const
     // the first will start sending back 3 satellites, the second just
     // stops responding when disabled.  This is not necessarily a good
     // thing.
-    if (instance == 1 && _sitl->gps_disable[instance]) {
+    if (instance == 1 && !_sitl->gps[instance].enabled) {
         return -1;
     }
 
-    const float byteloss = _sitl->gps_byteloss[instance];
+    const float byteloss = _sitl->gps[instance].byteloss;
 
     // shortcut if we're not doing byteloss:
     if (!is_positive(byteloss)) {
@@ -217,7 +323,7 @@ GPS_Backend::GPS_TOW GPS_Backend::gps_time()
 
 void GPS::check_backend_allocation()
 {
-    const Type configured_type = Type(_sitl->gps_type[instance].get());
+    const Type configured_type = Type(_sitl->gps[instance].type.get());
     if (allocated_type == configured_type) {
         return;
     }
@@ -328,7 +434,7 @@ void GPS::update()
 
     //Capture current position as basestation location for
     if (!_gps_has_basestation_position &&
-        now_ms >= _sitl->gps_lock_time[0]*1000UL) {
+        now_ms >= _sitl->gps[0].lock_time*1000UL) {
         _gps_basestation_data.latitude = latitude;
         _gps_basestation_data.longitude = longitude;
         _gps_basestation_data.altitude = altitude;
@@ -338,15 +444,15 @@ void GPS::update()
         _gps_has_basestation_position = true;
     }
 
-    const uint8_t idx = instance;  // alias to avoid code churn
+    const auto &params = _sitl->gps[instance];
 
     struct GPS_Data d {};
 
     // simulate delayed lock times
-    bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
+    bool have_lock = (params.enabled && now_ms >= params.lock_time*1000UL);
 
     // Only let physics run and GPS write at configured GPS rate (default 5Hz).
-    if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) {
+    if ((now_ms - last_write_update_ms) < (uint32_t)(1000/params.hertz)) {
         // Reading runs every iteration.
         // Beware- physics don't update every iteration with this approach.
         // Currently, none of the drivers rely on quickly updated physics.
@@ -356,37 +462,37 @@ void GPS::update()
 
     last_write_update_ms = now_ms;
 
-    d.num_sats = _sitl->gps_numsats[idx];
+    d.num_sats = params.numsats;
     d.latitude = latitude;
     d.longitude = longitude;
     d.yaw_deg = _sitl->state.yawDeg;
     d.roll_deg = _sitl->state.rollDeg;
     d.pitch_deg = _sitl->state.pitchDeg;
 
     // add an altitude error controlled by a slow sine wave
-    d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx];
+    d.altitude = altitude + params.noise * sinf(now_ms * 0.0005f) + params.alt_offset;
 
     // Add offset to c.g. velocity to get velocity at antenna and add simulated error
-    Vector3f velErrorNED = _sitl->gps_vel_err[idx];
+    Vector3f velErrorNED = params.vel_err;
     d.speedN = speedN + (velErrorNED.x * rand_float());
     d.speedE = speedE + (velErrorNED.y * rand_float());
     d.speedD = speedD + (velErrorNED.z * rand_float());
 
     d.have_lock = have_lock;
 
     // fill in accuracies
-    d.horizontal_acc = _sitl->gps_accuracy[idx];
-    d.vertical_acc = _sitl->gps_accuracy[idx];
-    d.speed_acc = _sitl->gps_vel_err[instance].get().xy().length();
+    d.horizontal_acc = params.accuracy;
+    d.vertical_acc = params.accuracy;
+    d.speed_acc = params.vel_err.get().xy().length();
 
-    if (_sitl->gps_drift_alt[idx] > 0) {
+    if (params.drift_alt > 0) {
         // add slow altitude drift controlled by a slow sine wave
-        d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f);
+        d.altitude += params.drift_alt*sinf(now_ms*0.001f*0.02f);
     }
 
     // correct the latitude, longitude, height and NED velocity for the offset between
     // the vehicle c.g. and GPS antenna
-    Vector3f posRelOffsetBF = _sitl->gps_pos_offset[idx];
+    Vector3f posRelOffsetBF = params.pos_offset;
     if (!posRelOffsetBF.is_zero()) {
         // get a rotation matrix following DCM conventions (body to earth)
         Matrix3f rotmat;
@@ -418,18 +524,18 @@ void GPS::update()
 
     // get delayed data
     d.timestamp_ms = now_ms;
-    d = interpolate_data(d, _sitl->gps_delay_ms[instance]);
+    d = interpolate_data(d, params.delay_ms);
 
     // Applying GPS glitch
     // Using first gps glitch
-    Vector3f glitch_offsets = _sitl->gps_glitch[idx];
+    Vector3f glitch_offsets = params.glitch;
     d.latitude += glitch_offsets.x;
     d.longitude += glitch_offsets.y;
     d.altitude += glitch_offsets.z;
 
-        if (_sitl->gps_jam[idx] == 1) {
-            simulate_jamming(d);
-        }
+    if (params.jam == 1) {
+        simulate_jamming(d);
+    }
 
     backend->publish(&d);
 }

libraries/SITL/SIM_GPS_NMEA.cpp

@@ -99,12 +99,12 @@ void GPS_NMEA::publish(const GPS_Data *d)
                      ground_track_deg,
                      dstring);
 
-    if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) {
+    if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_HDT) {
         nmea_printf("$GPHDT,%.2f,T", d->yaw_deg);
     }
-    else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) {
+    else if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_THS) {
         nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V');
-    } else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_KSXT) {
+    } else if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_KSXT) {
         // Unicore support
         // $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D
         nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,",