From fab1ef7a877e9431a92974d5cc17bdbfccb405fe Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 08:49:13 +1100 Subject: [PATCH] SITL: add ability to simulate more than 2 GPSs --- libraries/SITL/SIM_GPS.cpp | 164 ++++++++++-- libraries/SITL/SIM_GPS_NMEA.cpp | 6 +- libraries/SITL/SIM_GPS_UBLOX.cpp | 112 ++++----- libraries/SITL/SIM_GPS_UBLOX.h | 39 +++ libraries/SITL/SIM_config.h | 2 + libraries/SITL/SITL.cpp | 237 ++++-------------- libraries/SITL/SITL.h | 46 ++-- .../Params/Rolladen-Schneider-LS8b.param | 7 - .../Params/Schleicher-ASW27B.param | 23 +- 9 files changed, 330 insertions(+), 306 deletions(-) diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index d51521cf3b..6ca425a5ea 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -30,6 +30,121 @@ #include +namespace SITL { +// user settable parameters for GNSS sensors +const AP_Param::GroupInfo SIM::GPSParms::var_info[] = { + + // @Param: ENABLE + // @DisplayName: GPS enable + // @Description: Enable simulated GPS + // @Values: 0:Disable, 1:Enable + // @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: GLTCH + // @DisplayName: GPS Glitch + // @Description: Glitch offsets of simulated GPS sensor + // @Vector3Parameter: 1 + // @User: Advanced + AP_GROUPINFO("GLTCH", 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 GPS altitude error + // @Units: m + // @User: Advanced + AP_GROUPINFO("NOISE", 10, GPSParms, noise, 0), + + // @Param: LCKTIME + // @DisplayName: GPS Lock Time + // @Description: Delay in seconds before GPS 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 GPS output of NMEA heading HDT sentence or UBLOX_RELPOSNED + // @Values: 0:Disabled, 1:Emit HDT, 2:Emit THS, 3:KSXT, 4:Be Moving Baseline Base + // @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 +158,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 +200,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 +339,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 +450,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 +460,15 @@ void GPS::update() _gps_has_basestation_position = true; } - const uint8_t idx = instance; // alias to avoid code churn + const auto ¶ms = _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,7 +478,7 @@ 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; @@ -364,10 +486,10 @@ void GPS::update() 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()); @@ -375,18 +497,18 @@ void GPS::update() 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 +540,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); } diff --git a/libraries/SITL/SIM_GPS_NMEA.cpp b/libraries/SITL/SIM_GPS_NMEA.cpp index 23d2a91ad4..a4e19fb40b 100644 --- a/libraries/SITL/SIM_GPS_NMEA.cpp +++ b/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,,", diff --git a/libraries/SITL/SIM_GPS_UBLOX.cpp b/libraries/SITL/SIM_GPS_UBLOX.cpp index ce77a44d28..3f3ef5da68 100644 --- a/libraries/SITL/SIM_GPS_UBLOX.cpp +++ b/libraries/SITL/SIM_GPS_UBLOX.cpp @@ -35,6 +35,47 @@ void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) write_to_autopilot((char*)chk, sizeof(chk)); } +void GPS_UBlox::update_relposned(ubx_nav_relposned &relposned, uint32_t tow_ms, float yaw_deg) +{ + Vector3f ant1_pos { NaNf, NaNf, NaNf }; + + // find our partner: + for (uint8_t i=0; igps); i++) { + if (i == instance) { + // this shouldn't matter as our heading type should never be base + continue; + } + if (_sitl->gps[i].hdg_enabled != SITL::SIM::GPS_HEADING_BASE) { + continue; + } + ant1_pos = _sitl->gps[i].pos_offset.get(); + break; + } + if (ant1_pos.is_nan()) { + return; + } + + const Vector3f ant2_pos = _sitl->gps[instance].pos_offset.get(); + Vector3f rel_antenna_pos = ant2_pos - ant1_pos; + Matrix3f rot; + // project attitude back using gyros to get antenna orientation at time of GPS sample + Vector3f gyro(radians(_sitl->state.rollRate), + radians(_sitl->state.pitchRate), + radians(_sitl->state.yawRate)); + rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(yaw_deg)); + const float lag = _sitl->gps[instance].delay_ms * 0.001; + rot.rotate(gyro * (-lag)); + rel_antenna_pos = rot * rel_antenna_pos; + relposned.version = 1; + relposned.iTOW = tow_ms; + relposned.relPosN = rel_antenna_pos.x * 100; + relposned.relPosE = rel_antenna_pos.y * 100; + relposned.relPosD = rel_antenna_pos.z * 100; + relposned.relPosLength = rel_antenna_pos.length() * 100; + relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5; + relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid; +} + /* send a new set of GPS UBLOX packets */ @@ -139,44 +180,7 @@ void GPS_UBlox::publish(const GPS_Data *d) int32_t prRes; } sv[SV_COUNT]; } svinfo {}; - enum RELPOSNED { - gnssFixOK = 1U << 0, - diffSoln = 1U << 1, - relPosValid = 1U << 2, - carrSolnFloat = 1U << 3, - - carrSolnFixed = 1U << 4, - isMoving = 1U << 5, - refPosMiss = 1U << 6, - refObsMiss = 1U << 7, - - relPosHeadingValid = 1U << 8, - relPosNormalized = 1U << 9 - }; - struct PACKED ubx_nav_relposned { - uint8_t version; - uint8_t reserved1; - uint16_t refStationId; - uint32_t iTOW; - int32_t relPosN; - int32_t relPosE; - int32_t relPosD; - int32_t relPosLength; - int32_t relPosHeading; - uint8_t reserved2[4]; - int8_t relPosHPN; - int8_t relPosHPE; - int8_t relPosHPD; - int8_t relPosHPLength; - uint32_t accN; - uint32_t accE; - uint32_t accD; - uint32_t accLength; - uint32_t accHeading; - uint8_t reserved3[4]; - uint32_t flags; - } relposned {}; - + ubx_nav_relposned relposned {}; const uint8_t MSG_POSLLH = 0x2; const uint8_t MSG_STATUS = 0x3; const uint8_t MSG_DOP = 0x4; @@ -267,27 +271,15 @@ 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) { - 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; - Matrix3f rot; - // project attitude back using gyros to get antenna orientation at time of GPS sample - Vector3f gyro(radians(_sitl->state.rollRate), - radians(_sitl->state.pitchRate), - radians(_sitl->state.yawRate)); - rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw_deg)); - const float lag = _sitl->gps_delay_ms[instance] * 0.001; - rot.rotate(gyro * (-lag)); - rel_antenna_pos = rot * rel_antenna_pos; - relposned.version = 1; - relposned.iTOW = gps_tow.ms; - relposned.relPosN = rel_antenna_pos.x * 100; - relposned.relPosE = rel_antenna_pos.y * 100; - relposned.relPosD = rel_antenna_pos.z * 100; - relposned.relPosLength = rel_antenna_pos.length() * 100; - relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5; - relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid; + switch (_sitl->gps[instance].hdg_enabled) { + case SITL::SIM::GPS_HEADING_NONE: + case SITL::SIM::GPS_HEADING_BASE: + break; + case SITL::SIM::GPS_HEADING_THS: + case SITL::SIM::GPS_HEADING_KSXT: + case SITL::SIM::GPS_HEADING_HDT: + update_relposned(relposned, gps_tow.ms, d->yaw_deg); + break; } send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); @@ -296,7 +288,7 @@ void GPS_UBlox::publish(const GPS_Data *d) 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)); } diff --git a/libraries/SITL/SIM_GPS_UBLOX.h b/libraries/SITL/SIM_GPS_UBLOX.h index 6ab5d01eea..06f03cf5d8 100644 --- a/libraries/SITL/SIM_GPS_UBLOX.h +++ b/libraries/SITL/SIM_GPS_UBLOX.h @@ -15,6 +15,45 @@ public: void publish(const GPS_Data *d) override; private: + enum RELPOSNED { + gnssFixOK = 1U << 0, + diffSoln = 1U << 1, + relPosValid = 1U << 2, + carrSolnFloat = 1U << 3, + + carrSolnFixed = 1U << 4, + isMoving = 1U << 5, + refPosMiss = 1U << 6, + refObsMiss = 1U << 7, + + relPosHeadingValid = 1U << 8, + relPosNormalized = 1U << 9 + }; + struct PACKED ubx_nav_relposned { + uint8_t version; + uint8_t reserved1; + uint16_t refStationId; + uint32_t iTOW; + int32_t relPosN; + int32_t relPosE; + int32_t relPosD; + int32_t relPosLength; + int32_t relPosHeading; + uint8_t reserved2[4]; + int8_t relPosHPN; + int8_t relPosHPE; + int8_t relPosHPD; + int8_t relPosHPLength; + uint32_t accN; + uint32_t accE; + uint32_t accD; + uint32_t accLength; + uint32_t accHeading; + uint8_t reserved3[4]; + uint32_t flags; + }; + + void update_relposned(ubx_nav_relposned &relposned, uint32_t tow_ms, float yaw_deg); void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); }; diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 91956ad90c..557c3077ed 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -3,6 +3,8 @@ #include #include +#define AP_SIM_MAX_GPS_SENSORS 4 + #ifndef HAL_SIM_ADSB_ENABLED #define HAL_SIM_ADSB_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 1ff14520b7..e329ed232a 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -721,181 +721,38 @@ const AP_Param::GroupInfo SIM::var_info3[] = { #if HAL_SIM_GPS_ENABLED // GPS SITL parameters const AP_Param::GroupInfo SIM::var_gps[] = { - // @Param: GPS_DISABLE - // @DisplayName: GPS 1 disable - // @Description: Disables GPS 1 - // @Values: 0:Enable, 1:GPS Disabled - // @User: Advanced - AP_GROUPINFO("GPS_DISABLE", 1, SIM, gps_disable[0], 0), - // @Param: GPS_LAG_MS - // @DisplayName: GPS 1 Lag - // @Description: GPS 1 lag - // @Units: ms - // @User: Advanced - AP_GROUPINFO("GPS_LAG_MS", 2, SIM, gps_delay_ms[0], 100), - // @Param: GPS_TYPE - // @DisplayName: GPS 1 type - // @Description: Sets the type of simulation used for GPS 1 - // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP2, 11:Trimble, 19:MSP - // @User: Advanced - AP_GROUPINFO("GPS_TYPE", 3, SIM, gps_type[0], GPS::Type::UBLOX), - // @Param: GPS_BYTELOSS - // @DisplayName: GPS Byteloss - // @Description: Percent of bytes lost from GPS 1 - // @Units: % - // @User: Advanced - AP_GROUPINFO("GPS_BYTELOSS", 4, SIM, gps_byteloss[0], 0), - // @Param: GPS_NUMSATS - // @DisplayName: GPS 1 Num Satellites - // @Description: Number of satellites GPS 1 has in view - AP_GROUPINFO("GPS_NUMSATS", 5, SIM, gps_numsats[0], 10), - // @Param: GPS_GLITCH - // @DisplayName: GPS 1 Glitch - // @Description: Glitch offsets of simulated GPS 1 sensor - // @Vector3Parameter: 1 - // @User: Advanced - AP_GROUPINFO("GPS_GLITCH", 6, SIM, gps_glitch[0], 0), - // @Param: GPS_HZ - // @DisplayName: GPS 1 Hz - // @Description: GPS 1 Update rate - // @Units: Hz - AP_GROUPINFO("GPS_HZ", 7, SIM, gps_hertz[0], 5), - // @Param: GPS_DRIFTALT - // @DisplayName: GPS 1 Altitude Drift - // @Description: GPS 1 altitude drift error - // @Units: m - // @User: Advanced - AP_GROUPINFO("GPS_DRIFTALT", 8, SIM, gps_drift_alt[0], 0), - // @Param: GPS_POS - // @DisplayName: GPS 1 Position - // @Description: GPS 1 antenna phase center position relative to the body frame origin - // @Units: m - // @Vector3Parameter: 1 - AP_GROUPINFO("GPS_POS", 9, SIM, gps_pos_offset[0], 0), - // @Param: GPS_NOISE - // @DisplayName: GPS 1 Noise - // @Description: Amplitude of the GPS1 altitude error - // @Units: m - // @User: Advanced - AP_GROUPINFO("GPS_NOISE", 10, SIM, gps_noise[0], 0), - // @Param: GPS_LOCKTIME - // @DisplayName: GPS 1 Lock Time - // @Description: Delay in seconds before GPS1 acquires lock - // @Units: s - // @User: Advanced - AP_GROUPINFO("GPS_LOCKTIME", 11, SIM, gps_lock_time[0], 0), - // @Param: GPS_ALT_OFS - // @DisplayName: GPS 1 Altitude Offset - // @Description: GPS 1 Altitude Error - // @Units: m - AP_GROUPINFO("GPS_ALT_OFS", 12, SIM, gps_alt_offset[0], 0), - // @Param: GPS_HDG - // @DisplayName: GPS 1 Heading - // @Description: Enable GPS1 output of NMEA heading HDT sentence or UBLOX_RELPOSNED - // @Values: 0:Disabled, 1:Enabled - // @User: Advanced - AP_GROUPINFO("GPS_HDG", 13, SIM, gps_hdg_enabled[0], SIM::GPS_HEADING_NONE), - // @Param: GPS_ACC - // @DisplayName: GPS 1 Accuracy - // @Description: GPS 1 Accuracy - // @User: Advanced - AP_GROUPINFO("GPS_ACC", 14, SIM, gps_accuracy[0], 0.3), - // @Param: GPS_VERR - // @DisplayName: GPS 1 Velocity Error - // @Description: GPS 1 Velocity Error Offsets in NED - // @Vector3Parameter: 1 - // @User: Advanced - AP_GROUPINFO("GPS_VERR", 15, SIM, gps_vel_err[0], 0), - // @Param: GPS_JAM - // @DisplayName: GPS jamming enable - // @Description: Enable simulated GPS jamming - // @User: Advanced - // @Values: 0:Disabled, 1:Enabled - AP_GROUPINFO("GPS_JAM", 16, SIM, gps_jam[0], 0), - // @Param: GPS2_DISABLE - // @DisplayName: GPS 2 disable - // @Description: Disables GPS 2 - // @Values: 0:Enable, 1:GPS Disabled - // @User: Advanced - AP_GROUPINFO("GPS2_DISABLE", 30, SIM, gps_disable[1], 1), - // @Param: GPS2_LAG_MS - // @DisplayName: GPS 2 Lag - // @Description: GPS 2 lag in ms - // @Units: ms - // @User: Advanced - AP_GROUPINFO("GPS2_LAG_MS", 31, SIM, gps_delay_ms[1], 100), - // @Param: GPS2_TYPE - // @CopyFieldsFrom: SIM_GPS_TYPE - // @DisplayName: GPS 2 type - // @Description: Sets the type of simulation used for GPS 2 - AP_GROUPINFO("GPS2_TYPE", 32, SIM, gps_type[1], GPS::Type::UBLOX), - // @Param: GPS2_BYTELOS - // @DisplayName: GPS 2 Byteloss - // @Description: Percent of bytes lost from GPS 2 - // @Units: % - // @User: Advanced - AP_GROUPINFO("GPS2_BYTELOS", 33, SIM, gps_byteloss[1], 0), - // @Param: GPS2_NUMSATS - // @DisplayName: GPS 2 Num Satellites - // @Description: Number of satellites GPS 2 has in view - AP_GROUPINFO("GPS2_NUMSATS", 34, SIM, gps_numsats[1], 10), - // @Param: GPS2_GLTCH - // @DisplayName: GPS 2 Glitch - // @Description: Glitch offsets of simulated GPS 2 sensor - // @Vector3Parameter: 1 - // @User: Advanced - AP_GROUPINFO("GPS2_GLTCH", 35, SIM, gps_glitch[1], 0), - // @Param: GPS2_HZ - // @DisplayName: GPS 2 Hz - // @Description: GPS 2 Update rate - // @Units: Hz - AP_GROUPINFO("GPS2_HZ", 36, SIM, gps_hertz[1], 5), - // @Param: GPS2_DRFTALT - // @DisplayName: GPS 2 Altitude Drift - // @Description: GPS 2 altitude drift error - // @Units: m - // @User: Advanced - AP_GROUPINFO("GPS2_DRFTALT", 37, SIM, gps_drift_alt[1], 0), - // @Param: GPS2_POS - // @DisplayName: GPS 2 Position - // @Description: GPS 2 antenna phase center position relative to the body frame origin - // @Units: m - // @Vector3Parameter: 1 - AP_GROUPINFO("GPS2_POS", 38, SIM, gps_pos_offset[1], 0), - // @Param: GPS2_NOISE - // @DisplayName: GPS 2 Noise - // @Description: Amplitude of the GPS2 altitude error - // @Units: m - // @User: Advanced - AP_GROUPINFO("GPS2_NOISE", 39, SIM, gps_noise[1], 0), - // @Param: GPS2_LCKTIME - // @DisplayName: GPS 2 Lock Time - // @Description: Delay in seconds before GPS2 acquires lock - // @Units: s - // @User: Advanced - AP_GROUPINFO("GPS2_LCKTIME", 40, SIM, gps_lock_time[1], 0), - // @Param: GPS2_ALT_OFS - // @DisplayName: GPS 2 Altitude Offset - // @Description: GPS 2 Altitude Error - // @Units: m - AP_GROUPINFO("GPS2_ALT_OFS", 41, SIM, gps_alt_offset[1], 0), - // @Param: GPS2_HDG - // @DisplayName: GPS 2 Heading - // @Description: Enable GPS2 output of NMEA heading HDT sentence or UBLOX_RELPOSNED - // @Values: 0:Disabled, 1:Enabled - // @User: Advanced - AP_GROUPINFO("GPS2_HDG", 42, SIM, gps_hdg_enabled[1], SIM::GPS_HEADING_NONE), - // @Param: GPS2_ACC - // @DisplayName: GPS 2 Accuracy - // @Description: GPS 2 Accuracy - // @User: Advanced - AP_GROUPINFO("GPS2_ACC", 43, SIM, gps_accuracy[1], 0.3), - // @Param: GPS2_VERR - // @DisplayName: GPS 2 Velocity Error - // @Description: GPS 2 Velocity Error Offsets in NED - // @Vector3Parameter: 1 - // @User: Advanced - AP_GROUPINFO("GPS2_VERR", 44, SIM, gps_vel_err[1], 0), + // 1 was GPS_DISABLE + // 2 was GPS_LAG_MS + // 3 was GPS_TYPE + // 4 was GPS_BYTELOSS + // 5 was GPS_NUMSATS + // 6 was GPS_GLITCH + // 7 was GPS_HZ + // 8 was GPS_DRIFTALT + // 9 was GPS_POS + // 10 was GPS_NOISE + // 11 was GPS_LOCKTIME + // 12 was GPS_ALT_OFS + // 13 was GPS_HDG + // 14 was GPS_ACC + // 15 was GPS_VERR + // 16 was GPS_JAM + + // 30 was GPS2_DISABLE + // 31 was GPS2_LAG_MS + // 32 was GPS2_TYPE + // 33 was GPS2_BYTELOSS + // 34 was GPS2_NUMSATS + // 35 was GPS2_GLITCH + // 36 was GPS2_HZ + // 37 was GPS2_DRIFTALT + // 38 was GPS2_POS + // 39 was GPS2_NOISE + // 40 was GPS2_LOCKTIME + // 41 was GPS2_ALT_OFS + // 42 was GPS2_HDG + // 43 was GPS2_ACC + // 44 was GPS2_VERR // @Param: INIT_LAT_OFS // @DisplayName: Initial Latitude Offset @@ -915,14 +772,30 @@ const AP_Param::GroupInfo SIM::var_gps[] = { // @Description: Log number for GPS:update_file() AP_GROUPINFO("GPS_LOG_NUM", 48, SIM, gps_log_num, 0), - // @Param: GPS2_JAM - // @DisplayName: GPS jamming enable - // @Description: Enable simulated GPS jamming - // @User: Advanced - // @Values: 0:Disabled, 1:Enabled - AP_GROUPINFO("GPS2_JAM", 49, SIM, gps_jam[1], 0), + // 49 was GPS2_JAM - AP_GROUPEND +#if AP_SIM_MAX_GPS_SENSORS > 0 + // @Group: GPS1_ + // @Path: ./SIM_GPS.cpp + AP_SUBGROUPINFO(gps[0], "GPS1_", 50, SIM, GPSParms), +#endif +#if AP_SIM_MAX_GPS_SENSORS > 1 + // @Group: GPS2_ + // @Path: ./SIM_GPS.cpp + AP_SUBGROUPINFO(gps[1], "GPS2_", 51, SIM, GPSParms), +#endif +#if AP_SIM_MAX_GPS_SENSORS > 2 + // @Group: GPS3_ + // @Path: ./SIM_GPS.cpp + AP_SUBGROUPINFO(gps[2], "GPS3_", 52, SIM, GPSParms), +#endif +#if AP_SIM_MAX_GPS_SENSORS > 3 + // @Group: GPS4_ + // @Path: ./SIM_GPS.cpp + AP_SUBGROUPINFO(gps[3], "GPS4_", 53, SIM, GPSParms), +#endif + + AP_GROUPEND }; #endif // HAL_SIM_GPS_ENABLED diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index e23b2f6144..4edda5efd2 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -155,6 +155,7 @@ public: GPS_HEADING_HDT = 1, GPS_HEADING_THS = 2, GPS_HEADING_KSXT = 3, + GPS_HEADING_BASE = 4, // act as an RTK base }; struct sitl_fdm state; @@ -197,23 +198,6 @@ public: AP_Float engine_mul; // engine multiplier AP_Int8 engine_fail; // engine servo to fail (0-7) - AP_Float gps_noise[2]; // amplitude of the gps altitude error - AP_Int16 gps_lock_time[2]; // delay in seconds before GPS gets lock - AP_Int16 gps_alt_offset[2]; // gps alt error - AP_Int8 gps_disable[2]; // disable simulated GPS - AP_Int16 gps_delay_ms[2]; // delay in milliseconds - AP_Int8 gps_type[2]; // see enum SITL::GPS::Type - AP_Float gps_byteloss[2];// byte loss as a percent - AP_Int8 gps_numsats[2]; // number of visible satellites - AP_Vector3f gps_glitch[2]; // glitch offsets in lat, lon and altitude - AP_Int8 gps_hertz[2]; // GPS update rate in Hz - AP_Int8 gps_hdg_enabled[2]; // enable the output of a NMEA heading HDT sentence or UBLOX RELPOSNED - AP_Float gps_drift_alt[2]; // altitude drift error - AP_Vector3f gps_pos_offset[2]; // XYZ position of the GPS antenna phase centre relative to the body frame origin (m) - AP_Float gps_accuracy[2]; - AP_Vector3f gps_vel_err[2]; // Velocity error offsets in NED (x = N, y = E, z = D) - AP_Int8 gps_jam[2]; // jamming simulation enable - // initial offset on GPS lat/lon, used to shift origin AP_Float gps_init_lat_ofs; AP_Float gps_init_lon_ofs; @@ -311,7 +295,33 @@ public: AP_Float servo_filter; // servo 2p filter in Hz }; ServoParams servo; - + + class GPSParms { + public: + GPSParms(void) { + AP_Param::setup_object_defaults(this, var_info); + } + static const struct AP_Param::GroupInfo var_info[]; + + AP_Float noise; // amplitude of the gps altitude error + AP_Int16 lock_time; // delay in seconds before GPS gets lock + AP_Int16 alt_offset; // gps alt error + AP_Int8 enabled; // enable simulated GPS + AP_Int16 delay_ms; // delay in milliseconds + AP_Int8 type; // see enum SITL::GPS::Type + AP_Float byteloss;// byte loss as a percent + AP_Int8 numsats; // number of visible satellites + AP_Vector3f glitch; // glitch offsets in lat, lon and altitude + AP_Int8 hertz; // GPS update rate in Hz + AP_Int8 hdg_enabled; // enable the output of a NMEA heading HDT sentence or UBLOX RELPOSNED + AP_Float drift_alt; // altitude drift error + AP_Vector3f pos_offset; // XYZ position of the GPS antenna phase centre relative to the body frame origin (m) + AP_Float accuracy; + AP_Vector3f vel_err; // Velocity error offsets in NED (x = N, y = E, z = D) + AP_Int8 jam; // jamming simulation enable + }; + GPSParms gps[AP_SIM_MAX_GPS_SENSORS]; + // physics model parameters class ModelParm { public: diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index 4a349fd3e7..98d5e0b7a0 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -620,16 +620,9 @@ SIM_FLOW_POS_X,0 SIM_FLOW_POS_Y,0 SIM_FLOW_POS_Z,0 SIM_FLOW_RATE,10 -SIM_GP2_GLITCH_X,0 -SIM_GP2_GLITCH_Y,0 -SIM_GP2_GLITCH_Z,0 SIM_GPS_ALT_OFS,0 SIM_GPS_BYTELOSS,0 -SIM_GPS_DISABLE,0 SIM_GPS_DRIFTALT,0 -SIM_GPS_GLITCH_X,0 -SIM_GPS_GLITCH_Y,0 -SIM_GPS_GLITCH_Z,0 SIM_GPS_HZ,20 SIM_GPS_LOCKTIME,0 SIM_GPS_NOISE,0 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index bfaf562bcf..960c2c713c 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -640,21 +640,14 @@ SIM_FLOW_POS_X,0 SIM_FLOW_POS_Y,0 SIM_FLOW_POS_Z,0 SIM_FLOW_RATE,10 -SIM_GP2_GLITCH_X,0 -SIM_GP2_GLITCH_Y,0 -SIM_GP2_GLITCH_Z,0 -SIM_GPS_ALT_OFS,0 -SIM_GPS_BYTELOSS,0 -SIM_GPS_DISABLE,0 -SIM_GPS_DRIFTALT,0 -SIM_GPS_GLITCH_X,0 -SIM_GPS_GLITCH_Y,0 -SIM_GPS_GLITCH_Z,0 -SIM_GPS_HZ,20 -SIM_GPS_LOCKTIME,0 -SIM_GPS_NOISE,0 -SIM_GPS_NUMSATS,10 -SIM_GPS_TYPE,1 +SIM_GPS1_ALT_OFS,0 +SIM_GPS1_BYTELOSS,0 +SIM_GPS1_DRIFTALT,0 +SIM_GPS1_HZ,20 +SIM_GPS1_LOCKTIME,0 +SIM_GPS1_NOISE,0 +SIM_GPS1_NUMSATS,10 +SIM_GPS1_TYPE,1 SIM_GPS2_ENABLE,0 SIM_GPS2_TYPE,1 SIM_GRPE_ENABLE,0