SITL: add ability to simulate more than 2 GPSs

Author: peterbarker

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("DISABLE",    1, GPSParms, disable, 0),
+    // @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
 
@@ -78,11 +177,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].disable) {
         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 +316,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 +427,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 +437,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.disable && 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 +455,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 +517,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,,",

libraries/SITL/SIM_GPS_UBLOX.cpp

@@ -267,7 +267,8 @@ void GPS_UBlox::publish(const GPS_Data *d)
     pvt.headVeh = 0;
     memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2));
 
-    if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) {
+#if 0
+    if (_sitl->gps[instance].hdg_enabled > SITL::SIM::GPS_HEADING_NONE) {
         const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get();
         const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get();
         Vector3f rel_antenna_pos = ant2_pos - ant1_pos;
@@ -289,14 +290,15 @@ void GPS_UBlox::publish(const GPS_Data *d)
         relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5;
         relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid;
     }
+#endif
 
     send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
     send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
     send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
     send_ubx(MSG_SOL,    (uint8_t*)&sol, sizeof(sol));
     send_ubx(MSG_DOP,    (uint8_t*)&dop, sizeof(dop));
     send_ubx(MSG_PVT,    (uint8_t*)&pvt, sizeof(pvt));
-    if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) {
+    if (_sitl->gps[instance].hdg_enabled > SITL::SIM::GPS_HEADING_NONE) {
         send_ubx(MSG_RELPOSNED,    (uint8_t*)&relposned, sizeof(relposned));
     }
 

libraries/SITL/SIM_config.h

@@ -3,6 +3,8 @@
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <GCS_MAVLink/GCS_config.h>
 
+#define AP_SIM_MAX_GPS_SENSORS 4
+
 #ifndef HAL_SIM_ADSB_ENABLED
 #define HAL_SIM_ADSB_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif