From 1056b85e3c95b96e22d262df277b81fb33d02e88 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 08:49:12 +1100 Subject: [PATCH 1/6] AP_HAL_SITL: add ability to simulate more than 2 GPSs --- libraries/AP_HAL_SITL/SITL_State_common.cpp | 8 ++------ libraries/AP_HAL_SITL/SITL_State_common.h | 2 +- libraries/AP_HAL_SITL/UARTDriver.cpp | 4 ++-- 3 files changed, 5 insertions(+), 9 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 54a530bc447b1..47a625d4d7767 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/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]; diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 95ef129c1cce7..472f7dfefa660 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/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; diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index e809f4a43bb58..358fb61482412 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/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: From d15b92e37df41ca7f37b34f4dfa90f75fa811270 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 08:49:13 +1100 Subject: [PATCH 2/6] SITL: add ability to simulate more than 2 GPSs --- libraries/SITL/SIM_GPS.cpp | 148 +++++++++-- 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, 314 insertions(+), 306 deletions(-) diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index d51521cf3bff9..e925913f70c6e 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -30,6 +30,105 @@ #include +namespace SITL { +// user settable parameters for airspeed sensors +const AP_Param::GroupInfo SIM::GPSParms::var_info[] = { + // @Param: ENABLE + // @DisplayName: GPS enable + // @Description: Enabled 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 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 ¶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 +462,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 +470,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 +481,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 +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); } diff --git a/libraries/SITL/SIM_GPS_NMEA.cpp b/libraries/SITL/SIM_GPS_NMEA.cpp index 23d2a91ad45f9..a4e19fb40b468 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 ce77a44d284a1..3f3ef5da683b8 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 6ab5d01eea76f..06f03cf5d8e60 100644 --- a/libraries/SITL/SIM_GPS_UBLOX.h +++ b/libraries/SITL/SIM_GPS_UBLOX.h @@ -15,6 +15,45 @@ class GPS_UBlox : public GPS_Backend { 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 91956ad90c2ae..557c3077ed9de 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 1ff14520b72df..e329ed232a960 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 e23b2f6144791..5be3d352aaeba 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -155,6 +155,7 @@ class SIM { GPS_HEADING_HDT = 1, GPS_HEADING_THS = 2, GPS_HEADING_KSXT = 3, + GPS_HEADING_BASE = 4, // nb. I'm guessing, this could be ROVER! }; struct sitl_fdm state; @@ -197,23 +198,6 @@ class SIM { 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 @@ class SIM { 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 4a349fd3e72c4..98d5e0b7a06e4 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 bfaf562bcfabe..960c2c713c2c9 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 From 73399a36c28ccb1103dfd825e663bfb29dc4af98 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 08:49:13 +1100 Subject: [PATCH 3/6] Tools: add ability to simulate more than 2 GPSs --- Tools/autotest/arducopter.py | 100 +++++++++--------- Tools/autotest/arduplane.py | 36 +++---- .../autotest/default_params/airsim-quadX.parm | 2 +- .../copter-gps-for-yaw-nmea.parm | 4 +- .../default_params/copter-gps-for-yaw.parm | 5 +- .../default_params/tracker-gps-for-yaw.parm | 5 +- .../default_params/vee-gull_005.param | 19 ++-- Tools/autotest/quadplane.py | 4 +- Tools/autotest/rover.py | 2 +- Tools/autotest/vehicle_test_suite.py | 24 ++--- Tools/vagrant/mavinit.scr | 1 - 11 files changed, 98 insertions(+), 104 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 97792527db53d..b5755375aa651 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -552,13 +552,13 @@ def ThrottleFailsafe(self, side=60, timeout=360): self.start_subtest("Radio failsafe RTL fails into land mode due to bad position.") self.set_parameter('FS_THR_ENABLE', 1) self.takeoffAndMoveAway() - self.set_parameter('SIM_GPS_DISABLE', 1) + self.set_parameter('SIM_GPS1_ENABLE', 0) self.delay_sim_time(5) self.set_parameter("SIM_RC_FAIL", 1) self.wait_mode("LAND") self.wait_landed_and_disarmed() self.set_parameter("SIM_RC_FAIL", 0) - self.set_parameter('SIM_GPS_DISABLE', 0) + self.set_parameter('SIM_GPS1_ENABLE', 1) self.wait_ekf_happy() self.end_subtest("Completed Radio failsafe RTL fails into land mode due to bad position.") @@ -567,13 +567,13 @@ def ThrottleFailsafe(self, side=60, timeout=360): self.start_subtest("Radio failsafe SmartRTL->RTL fails into land mode due to bad position.") self.set_parameter('FS_THR_ENABLE', 4) self.takeoffAndMoveAway() - self.set_parameter('SIM_GPS_DISABLE', 1) + self.set_parameter('SIM_GPS1_ENABLE', 0) self.delay_sim_time(5) self.set_parameter("SIM_RC_FAIL", 1) self.wait_mode("LAND") self.wait_landed_and_disarmed() self.set_parameter("SIM_RC_FAIL", 0) - self.set_parameter('SIM_GPS_DISABLE', 0) + self.set_parameter('SIM_GPS1_ENABLE', 1) self.wait_ekf_happy() self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into land mode due to bad position.") @@ -582,13 +582,13 @@ def ThrottleFailsafe(self, side=60, timeout=360): self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to bad position.") self.set_parameter('FS_THR_ENABLE', 5) self.takeoffAndMoveAway() - self.set_parameter('SIM_GPS_DISABLE', 1) + self.set_parameter('SIM_GPS1_ENABLE', 0) self.delay_sim_time(5) self.set_parameter("SIM_RC_FAIL", 1) self.wait_mode("LAND") self.wait_landed_and_disarmed() self.set_parameter("SIM_RC_FAIL", 0) - self.set_parameter('SIM_GPS_DISABLE', 0) + self.set_parameter('SIM_GPS1_ENABLE', 1) self.wait_ekf_happy() self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to bad position.") @@ -597,9 +597,9 @@ def ThrottleFailsafe(self, side=60, timeout=360): self.start_subtest("Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.") self.set_parameter('FS_THR_ENABLE', 4) self.takeoffAndMoveAway() - self.set_parameter('SIM_GPS_DISABLE', 1) + self.set_parameter('SIM_GPS1_ENABLE', 0) self.wait_statustext("SmartRTL deactivated: bad position", timeout=60) - self.set_parameter('SIM_GPS_DISABLE', 0) + self.set_parameter('SIM_GPS1_ENABLE', 1) self.wait_ekf_happy() self.delay_sim_time(5) self.set_parameter("SIM_RC_FAIL", 1) @@ -613,9 +613,9 @@ def ThrottleFailsafe(self, side=60, timeout=360): self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to no path.") self.set_parameter('FS_THR_ENABLE', 5) self.takeoffAndMoveAway() - self.set_parameter('SIM_GPS_DISABLE', 1) + self.set_parameter('SIM_GPS1_ENABLE', 0) self.wait_statustext("SmartRTL deactivated: bad position", timeout=60) - self.set_parameter('SIM_GPS_DISABLE', 0) + self.set_parameter('SIM_GPS1_ENABLE', 1) self.wait_ekf_happy() self.delay_sim_time(5) self.set_parameter("SIM_RC_FAIL", 1) @@ -1958,8 +1958,8 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20): glitch_current = 0 self.progress("Apply first glitch") self.set_parameters({ - "SIM_GPS_GLITCH_X": glitch_lat[glitch_current], - "SIM_GPS_GLITCH_Y": glitch_lon[glitch_current], + "SIM_GPS1_GLTCH_X": glitch_lat[glitch_current], + "SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current], }) # record position for 30 seconds @@ -1973,15 +1973,15 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20): glitch_current = -1 self.progress("Completed Glitches") self.set_parameters({ - "SIM_GPS_GLITCH_X": 0, - "SIM_GPS_GLITCH_Y": 0, + "SIM_GPS1_GLTCH_X": 0, + "SIM_GPS1_GLTCH_Y": 0, }) else: self.progress("Applying glitch %u" % glitch_current) # move onto the next glitch self.set_parameters({ - "SIM_GPS_GLITCH_X": glitch_lat[glitch_current], - "SIM_GPS_GLITCH_Y": glitch_lon[glitch_current], + "SIM_GPS1_GLTCH_X": glitch_lat[glitch_current], + "SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current], }) # start displaying distance moved after all glitches applied @@ -2002,8 +2002,8 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20): # disable gps glitch if glitch_current != -1: self.set_parameters({ - "SIM_GPS_GLITCH_X": 0, - "SIM_GPS_GLITCH_Y": 0, + "SIM_GPS1_GLTCH_X": 0, + "SIM_GPS1_GLTCH_Y": 0, }) if self.use_map: self.show_gps_and_sim_positions(False) @@ -2024,7 +2024,7 @@ def GPSGlitchLoiter2(self): self.wait_attitude(desroll=0, despitch=0, timeout=10, tolerance=1) # apply glitch - self.set_parameter("SIM_GPS_GLITCH_X", 0.001) + self.set_parameter("SIM_GPS1_GLTCH_X", 0.001) # check lean angles remain stable for 20 seconds tstart = self.get_sim_time() @@ -2098,11 +2098,11 @@ def GPSGlitchAuto(self, timeout=180): # stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode self.change_mode("LOITER") self.set_parameters({ - "SIM_GPS_DISABLE": 1, + "SIM_GPS1_ENABLE": 0, }) self.delay_sim_time(2) self.set_parameters({ - "SIM_GPS_DISABLE": 0, + "SIM_GPS1_ENABLE": 1, }) # regaining GPS should not result in it falling back to a non-navigation mode self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1) @@ -2118,8 +2118,8 @@ def GPSGlitchAuto(self, timeout=180): glitch_current = 0 self.progress("Apply first glitch") self.set_parameters({ - "SIM_GPS_GLITCH_X": glitch_lat[glitch_current], - "SIM_GPS_GLITCH_Y": glitch_lon[glitch_current], + "SIM_GPS1_GLTCH_X": glitch_lat[glitch_current], + "SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current], }) # record position for 30 seconds @@ -2132,15 +2132,15 @@ def GPSGlitchAuto(self, timeout=180): if glitch_current < glitch_num: self.progress("Applying glitch %u" % glitch_current) self.set_parameters({ - "SIM_GPS_GLITCH_X": glitch_lat[glitch_current], - "SIM_GPS_GLITCH_Y": glitch_lon[glitch_current], + "SIM_GPS1_GLTCH_X": glitch_lat[glitch_current], + "SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current], }) # turn off glitching self.progress("Completed Glitches") self.set_parameters({ - "SIM_GPS_GLITCH_X": 0, - "SIM_GPS_GLITCH_Y": 0, + "SIM_GPS1_GLTCH_X": 0, + "SIM_GPS1_GLTCH_Y": 0, }) # continue with the mission @@ -2526,7 +2526,7 @@ def OpticalFlowLimits(self): self.set_parameters({ "SIM_FLOW_ENABLE": 1, "FLOW_TYPE": 10, - "SIM_GPS_DISABLE": 1, + "SIM_GPS1_ENABLE": 0, "SIM_TERRAIN": 0, }) @@ -2980,13 +2980,13 @@ def FarOrigin(self): '''fly a mission far from the vehicle origin''' # Fly mission #1 self.set_parameters({ - "SIM_GPS_DISABLE": 1, + "SIM_GPS1_ENABLE": 0, }) self.reboot_sitl() nz = mavutil.location(-43.730171, 169.983118, 1466.3, 270) self.set_origin(nz) self.set_parameters({ - "SIM_GPS_DISABLE": 0, + "SIM_GPS1_ENABLE": 1, }) self.progress("# Load copter_mission") # load the waypoint count @@ -3028,8 +3028,8 @@ def CANGPSCopterMission(self): "GPS1_TYPE": 9, "GPS2_TYPE": 9, # disable simulated GPS, so only via DroneCAN - "SIM_GPS_DISABLE": 1, - "SIM_GPS2_DISABLE": 1, + "SIM_GPS1_ENABLE": 0, + "SIM_GPS2_ENABLE": 0, # this ensures we use DroneCAN baro and compass "SIM_BARO_COUNT" : 0, "SIM_MAG1_DEVID" : 0, @@ -3206,7 +3206,7 @@ def GuidedEKFLaneChange(self): "EK3_SRC1_POSZ": 3, "EK3_AFFINITY" : 1, "GPS2_TYPE" : 1, - "SIM_GPS2_DISABLE" : 0, + "SIM_GPS2_ENABLE" : 1, "SIM_GPS2_GLTCH_Z" : -30 }) self.reboot_sitl() @@ -7245,7 +7245,7 @@ def loiter_requires_position(self): self.context_push() self.set_parameters({ "GPS1_TYPE": 2, - "SIM_GPS_DISABLE": 1, + "SIM_GPS1_ENABLE": 0, }) # if there is no GPS at all then we must direct EK3 to not use # it at all. Otherwise it will never initialise, as it wants @@ -8986,7 +8986,7 @@ def test_replay_gps_bit(self): "RNGFND1_POS_Y": -0.07, "RNGFND1_POS_Z": -0.005, "SIM_SONAR_SCALE": 30, - "SIM_GPS2_DISABLE": 0, + "SIM_GPS2_ENABLE": 1, }) self.reboot_sitl() @@ -9045,7 +9045,7 @@ def GPSBlendingLog(self): self.set_parameters({ "GPS2_TYPE": 1, "SIM_GPS2_TYPE": 1, - "SIM_GPS2_DISABLE": 0, + "SIM_GPS2_ENABLE": 1, "GPS_AUTO_SWITCH": 2, }) self.reboot_sitl() @@ -9118,9 +9118,9 @@ def GPSBlending(self): "WP_YAW_BEHAVIOR": 0, # do not yaw "GPS2_TYPE": 1, "SIM_GPS2_TYPE": 1, - "SIM_GPS2_DISABLE": 0, - "SIM_GPS_POS_X": 1.0, - "SIM_GPS_POS_Y": -1.0, + "SIM_GPS2_ENABLE": 1, + "SIM_GPS1_POS_X": 1.0, + "SIM_GPS1_POS_Y": -1.0, "SIM_GPS2_POS_X": -1.0, "SIM_GPS2_POS_Y": 1.0, "GPS_AUTO_SWITCH": 2, @@ -9178,9 +9178,9 @@ def GPSWeightedBlending(self): "WP_YAW_BEHAVIOR": 0, # do not yaw "GPS2_TYPE": 1, "SIM_GPS2_TYPE": 1, - "SIM_GPS2_DISABLE": 0, - "SIM_GPS_POS_X": 1.0, - "SIM_GPS_POS_Y": -1.0, + "SIM_GPS2_ENABLE": 1, + "SIM_GPS1_POS_X": 1.0, + "SIM_GPS1_POS_Y": -1.0, "SIM_GPS2_POS_X": -1.0, "SIM_GPS2_POS_Y": 1.0, "GPS_AUTO_SWITCH": 2, @@ -9188,8 +9188,8 @@ def GPSWeightedBlending(self): # configure velocity errors such that the 1st GPS should be # 4/5, second GPS 1/5 of result (0.5**2)/((0.5**2)+(1.0**2)) self.set_parameters({ - "SIM_GPS_VERR_X": 0.3, # m/s - "SIM_GPS_VERR_Y": 0.4, + "SIM_GPS1_VERR_X": 0.3, # m/s + "SIM_GPS1_VERR_Y": 0.4, "SIM_GPS2_VERR_X": 0.6, # m/s "SIM_GPS2_VERR_Y": 0.8, "GPS_BLEND_MASK": 4, # use only speed for blend calculations @@ -9245,9 +9245,9 @@ def GPSBlendingAffinity(self): "WP_YAW_BEHAVIOR": 0, # do not yaw "GPS2_TYPE": 1, "SIM_GPS2_TYPE": 1, - "SIM_GPS2_DISABLE": 0, - "SIM_GPS_POS_X": 1.0, - "SIM_GPS_POS_Y": -1.0, + "SIM_GPS2_ENABLE": 1, + "SIM_GPS1_POS_X": 1.0, + "SIM_GPS1_POS_Y": -1.0, "SIM_GPS2_POS_X": -1.0, "SIM_GPS2_POS_Y": 1.0, "GPS_AUTO_SWITCH": 2, @@ -11754,7 +11754,7 @@ def GuidedForceArm(self): '''ensure Guided acts appropriately when force-armed''' self.set_parameters({ "EK3_SRC2_VELXY": 5, - "SIM_GPS_DISABLE": 1, + "SIM_GPS1_ENABLE": 0, }) self.load_default_params_file("copter-optflow.parm") self.reboot_sitl() @@ -11941,7 +11941,7 @@ def REQUIRE_POSITION_FOR_ARMING(self): '''check FlightOption::REQUIRE_POSITION_FOR_ARMING works''' self.context_push() self.set_parameters({ - "SIM_GPS_NUMSATS": 3, # EKF does not like < 6 + "SIM_GPS1_NUMSATS": 3, # EKF does not like < 6 }) self.reboot_sitl() self.change_mode('STABILIZE') @@ -12142,7 +12142,7 @@ def CommonOrigin(self): # switch to EKF3 self.set_parameters({ - 'SIM_GPS_GLITCH_X' : 0.001, # about 100m + 'SIM_GPS1_GLTCH_X' : 0.001, # about 100m 'EK3_CHECK_SCALE' : 100, 'AHRS_EKF_TYPE' : 3}) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index bc0d157258f51..b13d465882a8c 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -185,7 +185,7 @@ def fly_RTL(self): def NeedEKFToArm(self): """Ensure the EKF must be healthy for the vehicle to arm.""" self.progress("Ensuring we need EKF to be healthy to arm") - self.set_parameter("SIM_GPS_DISABLE", 1) + self.set_parameter("SIM_GPS1_ENABLE", 0) self.context_collect("STATUSTEXT") tstart = self.get_sim_time() success = False @@ -201,7 +201,7 @@ def NeedEKFToArm(self): except AutoTestTimeoutException: pass - self.set_parameter("SIM_GPS_DISABLE", 0) + self.set_parameter("SIM_GPS1_ENABLE", 1) self.wait_ready_to_arm() def fly_LOITER(self, num_circles=4): @@ -2046,14 +2046,14 @@ def deadreckoning_main(self, disable_airspeed_sensor=False): self.delay_sim_time(60) else: self.delay_sim_time(20) - self.set_parameter("SIM_GPS_DISABLE", 1) + self.set_parameter("SIM_GPS1_ENABLE", 0) self.progress("Continue orbit without GPS") self.delay_sim_time(20) self.change_mode("RTL") self.wait_distance_to_home(100, 200, timeout=200) # go into LOITER to create additonal time for a GPS re-enable test self.change_mode("LOITER") - self.set_parameter("SIM_GPS_DISABLE", 0) + self.set_parameter("SIM_GPS1_ENABLE", 1) t_enabled = self.get_sim_time() # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning # to prevent bad GPS being used when coming back after loss of lock due to interence. @@ -2064,9 +2064,9 @@ def deadreckoning_main(self, disable_airspeed_sensor=False): self.delay_sim_time(20) self.set_parameter("AHRS_OPTIONS", 1) - self.set_parameter("SIM_GPS_JAM", 1) + self.set_parameter("SIM_GPS1_JAM", 1) self.delay_sim_time(10) - self.set_parameter("SIM_GPS_JAM", 0) + self.set_parameter("SIM_GPS1_JAM", 0) t_enabled = self.get_sim_time() # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning # to prevent bad GPS being used when coming back after loss of lock due to interence. @@ -3325,7 +3325,7 @@ def EKFlaneswitch(self): "EK3_AFFINITY": 15, # enable affinity for all sensors "EK3_IMU_MASK": 3, # use only 2 IMUs "GPS2_TYPE": 1, - "SIM_GPS2_DISABLE": 0, + "SIM_GPS2_ENABLE": 1, "SIM_BARO_COUNT": 2, "SIM_BAR2_DISABLE": 0, "ARSPD2_TYPE": 2, @@ -3400,9 +3400,9 @@ def statustext_hook(mav, message): # noise on each axis def sim_gps_verr(): self.set_parameters({ - "SIM_GPS_VERR_X": self.get_parameter("SIM_GPS_VERR_X") + 2, - "SIM_GPS_VERR_Y": self.get_parameter("SIM_GPS_VERR_Y") + 2, - "SIM_GPS_VERR_Z": self.get_parameter("SIM_GPS_VERR_Z") + 2, + "SIM_GPS1_VERR_X": self.get_parameter("SIM_GPS1_VERR_X") + 2, + "SIM_GPS1_VERR_Y": self.get_parameter("SIM_GPS1_VERR_Y") + 2, + "SIM_GPS1_VERR_Z": self.get_parameter("SIM_GPS1_VERR_Z") + 2, }) self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=sim_gps_verr, check_context=True) if self.lane_switches != [1, 0, 1]: @@ -4842,8 +4842,8 @@ def DCMFallback(self): self.context_collect('STATUSTEXT') self.set_parameters({ "EK3_POS_I_GATE": 0, - "SIM_GPS_HZ": 1, - "SIM_GPS_LAG_MS": 1000, + "SIM_GPS1_HZ": 1, + "SIM_GPS1_LAG_MS": 1000, }) self.wait_statustext("DCM Active", check_context=True, timeout=60) self.wait_statustext("EKF3 Active", check_context=True) @@ -5502,8 +5502,8 @@ def MissionJumpTags(self): def AltResetBadGPS(self): '''Tests the handling of poor GPS lock pre-arm alt resets''' self.set_parameters({ - "SIM_GPS_GLITCH_Z": 0, - "SIM_GPS_ACC": 0.3, + "SIM_GPS1_GLTCH_Z": 0, + "SIM_GPS1_ACC": 0.3, }) self.wait_ready_to_arm() @@ -5513,8 +5513,8 @@ def AltResetBadGPS(self): raise NotAchievedException("Bad relative alt %.1f" % relalt) self.progress("Setting low accuracy, glitching GPS") - self.set_parameter("SIM_GPS_ACC", 40) - self.set_parameter("SIM_GPS_GLITCH_Z", -47) + self.set_parameter("SIM_GPS1_ACC", 40) + self.set_parameter("SIM_GPS1_GLTCH_Z", -47) self.progress("Waiting 10s for height update") self.delay_sim_time(10) @@ -6091,7 +6091,7 @@ def GuidedAttitudeNoGPS(self): self.takeoff(50) self.change_mode('GUIDED') self.context_push() - self.set_parameter('SIM_GPS_DISABLE', 1) + self.set_parameter('SIM_GPS1_ENABLE', 0) self.delay_sim_time(30) self.set_attitude_target() self.context_pop() @@ -6168,7 +6168,7 @@ def SetHomeAltChange(self): def ForceArm(self): '''check force-arming functionality''' - self.set_parameter("SIM_GPS_DISABLE", 1) + self.set_parameter("SIM_GPS1_ENABLE", 0) # 21196 is the mavlink standard, 2989 is legacy for magic_value in 21196, 2989: self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK, diff --git a/Tools/autotest/default_params/airsim-quadX.parm b/Tools/autotest/default_params/airsim-quadX.parm index a8987896f8a84..c0636c5205c37 100644 --- a/Tools/autotest/default_params/airsim-quadX.parm +++ b/Tools/autotest/default_params/airsim-quadX.parm @@ -18,7 +18,7 @@ WPNAV_SPEED_DN 300 WPNAV_SPEED 2000 RTL_ALT 2500 ANGLE_MAX 3000 -SIM_GPS_LAG_MS 0 +SIM_GPS1_LAG_MS 0 GPS_DELAY_MS 20 INS_GYRO_FILTER 50 INS_ACCEL_FILTER 50 diff --git a/Tools/autotest/default_params/copter-gps-for-yaw-nmea.parm b/Tools/autotest/default_params/copter-gps-for-yaw-nmea.parm index 41af6fa868b91..6ec6add79623f 100644 --- a/Tools/autotest/default_params/copter-gps-for-yaw-nmea.parm +++ b/Tools/autotest/default_params/copter-gps-for-yaw-nmea.parm @@ -1,5 +1,5 @@ # SITL GPS-for-yaw using a single simulated NMEA GPS EK3_SRC1_YAW 2 GPS1_TYPE 5 -SIM_GPS_TYPE 5 -SIM_GPS_HDG 1 +SIM_GPS1_TYPE 5 +SIM_GPS1_HDG 1 diff --git a/Tools/autotest/default_params/copter-gps-for-yaw.parm b/Tools/autotest/default_params/copter-gps-for-yaw.parm index 231fb84e8ecec..1e54532f4f6b1 100644 --- a/Tools/autotest/default_params/copter-gps-for-yaw.parm +++ b/Tools/autotest/default_params/copter-gps-for-yaw.parm @@ -5,7 +5,8 @@ GPS1_TYPE 17 GPS2_TYPE 18 GPS1_POS_Y -0.2 GPS2_POS_Y 0.2 -SIM_GPS_POS_Y -0.2 +SIM_GPS1_POS_Y -0.2 SIM_GPS2_POS_Y 0.2 -SIM_GPS2_DISABLE 0 +SIM_GPS2_ENABLE 1 SIM_GPS2_HDG 1 +SIM_GPS1_HDG 4 diff --git a/Tools/autotest/default_params/tracker-gps-for-yaw.parm b/Tools/autotest/default_params/tracker-gps-for-yaw.parm index 231fb84e8ecec..1e54532f4f6b1 100644 --- a/Tools/autotest/default_params/tracker-gps-for-yaw.parm +++ b/Tools/autotest/default_params/tracker-gps-for-yaw.parm @@ -5,7 +5,8 @@ GPS1_TYPE 17 GPS2_TYPE 18 GPS1_POS_Y -0.2 GPS2_POS_Y 0.2 -SIM_GPS_POS_Y -0.2 +SIM_GPS1_POS_Y -0.2 SIM_GPS2_POS_Y 0.2 -SIM_GPS2_DISABLE 0 +SIM_GPS2_ENABLE 1 SIM_GPS2_HDG 1 +SIM_GPS1_HDG 4 diff --git a/Tools/autotest/default_params/vee-gull_005.param b/Tools/autotest/default_params/vee-gull_005.param index 7a2e5bf48474d..ab340c7eaf020 100644 --- a/Tools/autotest/default_params/vee-gull_005.param +++ b/Tools/autotest/default_params/vee-gull_005.param @@ -628,19 +628,12 @@ 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_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,5 -SIM_GPS_NOISE,0 -SIM_GPS_NUMSATS,10 -SIM_GPS_TYPE,1 +SIM_GPS1_BYTELOSS,0 +SIM_GPS1_DRIFTALT,0 +SIM_GPS1_HZ,5 +SIM_GPS1_NOISE,0 +SIM_GPS1_NUMSATS,10 +SIM_GPS1_TYPE,1 SIM_GPS2_ENABLE,0 SIM_IMU_POS_X,0 SIM_IMU_POS_Y,0 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 0e6ef03d3f6c2..7296e349451a4 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1764,8 +1764,8 @@ def DCMClimbRate(self): # Kill any GPSs self.set_parameters({ - 'SIM_GPS_DISABLE': 1, - 'SIM_GPS2_DISABLE': 1, + 'SIM_GPS1_ENABLE': 0, + 'SIM_GPS2_ENABLE': 0, }) self.delay_sim_time(5) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 46ec91abf99c9..738677041b5a9 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6807,7 +6807,7 @@ def JammingSimulation(self): '''Test jamming simulation works''' self.wait_ready_to_arm() start_loc = self.assert_receive_message('GPS_RAW_INT') - self.set_parameter("SIM_GPS_JAM", 1) + self.set_parameter("SIM_GPS1_JAM", 1) class Requirement(): def __init__(self, field, min_value): diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 5885c21267fc9..0d21fe890fc1d 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -3665,7 +3665,7 @@ def HIGH_LATENCY2(self): if (m.failure_flags & mavutil.mavlink.HL_FAILURE_FLAG_GPS) != 0: raise NotAchievedException("Expected GPS to be OK") self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_GPS, True, True, True) - self.set_parameter("SIM_GPS_TYPE", 0) + self.set_parameter("SIM_GPS1_TYPE", 0) self.delay_sim_time(10) self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_GPS, False, False, False) m = self.poll_message("HIGH_LATENCY2") @@ -3674,7 +3674,7 @@ def HIGH_LATENCY2(self): raise NotAchievedException("Expected GPS to be failed") self.start_subtest("HIGH_LATENCY2 location") - self.set_parameter("SIM_GPS_TYPE", 1) + self.set_parameter("SIM_GPS1_TYPE", 1) self.delay_sim_time(10) m = self.poll_message("HIGH_LATENCY2") self.progress(self.dump_message_verbose(m)) @@ -8471,7 +8471,7 @@ def wait_ekf_flags(self, required_value, error_bits, **kwargs): def wait_gps_disable(self, position_horizontal=True, position_vertical=False, timeout=30): """Disable GPS and wait for EKF to report the end of assistance from GPS.""" - self.set_parameter("SIM_GPS_DISABLE", 1) + self.set_parameter("SIM_GPS1_ENABLE", 0) tstart = self.get_sim_time() """ if using SITL estimates directly """ @@ -10683,7 +10683,7 @@ def ArmFeatures(self): p1=1, # ARM want_result=mavutil.mavlink.MAV_RESULT_FAILED, ) - self.set_parameter("SIM_GPS_DISABLE", 0) + self.set_parameter("SIM_GPS1_ENABLE", 1) self.wait_ekf_happy() # EKF may stay unhappy for a while self.progress("PASS not able to arm without Position in mode : %s" % mode) if mode in self.get_no_position_not_settable_modes_list(): @@ -10693,10 +10693,10 @@ def ArmFeatures(self): try: self.change_mode(mode, timeout=15) except AutoTestTimeoutException: - self.set_parameter("SIM_GPS_DISABLE", 0) + self.set_parameter("SIM_GPS1_ENABLE", 1) self.progress("PASS not able to set mode without Position : %s" % mode) except ValueError: - self.set_parameter("SIM_GPS_DISABLE", 0) + self.set_parameter("SIM_GPS1_ENABLE", 1) self.progress("PASS not able to set mode without Position : %s" % mode) if mode == "FOLLOW": self.set_parameter("FOLL_ENABLE", 0) @@ -12531,7 +12531,7 @@ def AdvancedFailsafe(self): self.context_collect("STATUSTEXT") self.set_parameters({ "AFS_MAX_GPS_LOSS": 1, - "SIM_GPS_DISABLE": 1, + "SIM_GPS1_ENABLE": 0, }) self.wait_statustext("AFS State: GPS_LOSS", check_context=True) self.context_pop() @@ -14160,7 +14160,7 @@ def GPSTypes(self): self.context_collect("STATUSTEXT") for (sim_gps_type, name, gps_type, detect_name, serial_protocol, detect_prefix) in sim_gps: self.start_subtest("Checking GPS type %s" % name) - self.set_parameter("SIM_GPS_TYPE", sim_gps_type) + self.set_parameter("SIM_GPS1_TYPE", sim_gps_type) self.set_parameter("SERIAL3_PROTOCOL", serial_protocol) if gps_type is None: gps_type = 1 # auto-detect @@ -14265,7 +14265,7 @@ def MultipleGPS(self): self.start_subtest("Ensure detection when sim gps connected") self.set_parameter("SIM_GPS2_TYPE", 1) - self.set_parameter("SIM_GPS2_DISABLE", 0) + self.set_parameter("SIM_GPS2_ENABLE", 1) # a reboot is required after setting GPS2_TYPE. We start # sending GPS2_RAW out, once the parameter is set, but a # reboot is required because _port[1] is only set in @@ -14281,9 +14281,9 @@ def MultipleGPS(self): raise NotAchievedException("Incorrect fix type") self.start_subtest("Check parameters are per-GPS") - self.assert_parameter_value("SIM_GPS_NUMSATS", 10) + self.assert_parameter_value("SIM_GPS1_NUMSATS", 10) self.assert_gps_satellite_count("GPS_RAW_INT", 10) - self.set_parameter("SIM_GPS_NUMSATS", 13) + self.set_parameter("SIM_GPS1_NUMSATS", 13) self.assert_gps_satellite_count("GPS_RAW_INT", 13) self.assert_parameter_value("SIM_GPS2_NUMSATS", 10) @@ -14311,7 +14311,7 @@ def MultipleGPS(self): if abs(gpi_alt - new_gpi_alt) > 100: raise NotAchievedException("alt moved unexpectedly") self.progress("Killing first GPS") - self.set_parameter("SIM_GPS_DISABLE", 1) + self.set_parameter("SIM_GPS1_ENABLE", 0) self.delay_sim_time(1) self.progress("Checking altitude now matches second GPS") m = self.assert_receive_message("GLOBAL_POSITION_INT") diff --git a/Tools/vagrant/mavinit.scr b/Tools/vagrant/mavinit.scr index 4b56c421c538b..fe7027ba0def7 100644 --- a/Tools/vagrant/mavinit.scr +++ b/Tools/vagrant/mavinit.scr @@ -17,7 +17,6 @@ module load graph @alias add odroidpower relay set 0 @alias add neutral2 servo set 12 1500 @alias add ekf param set AHRS_EKF_USE -@alias add gpsdisable param set SIM_GPS_DISABLE @alias add bb status gps* scaled* raw* @alias add g graph @alias add grc3 g RC_CHANNELS.chan3_raw SERVO_OUTPUT_RAW.servo3_raw From 2a254b528bfae52cd10e6eaef2da688a3d245f66 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 07:22:59 +1100 Subject: [PATCH 4/6] autotest: increase debug when retrying parameter download --- Tools/autotest/vehicle_test_suite.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 0d21fe890fc1d..dddd4019d87b9 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -12313,6 +12313,7 @@ def download_parameters(self, target_system, target_component): expected_count = 0 seen_ids = {} self.progress("Downloading parameters") + debug = False while True: now = self.get_sim_time_cached() if not start_done or now - last_parameter_received > 10: @@ -12323,6 +12324,7 @@ def download_parameters(self, target_system, target_component): elif attempt_count != 0: self.progress("Download failed; retrying") self.delay_sim_time(1) + debug = True self.drain_mav() self.mav.mav.param_request_list_send(target_system, target_component) attempt_count += 1 @@ -12337,8 +12339,8 @@ def download_parameters(self, target_system, target_component): if m.param_index == 65535: self.progress("volunteered parameter: %s" % str(m)) continue - if False: - self.progress(" received (%4u/%4u %s=%f" % + if debug: + self.progress(" received id=%4u param_count=%4u %s=%f" % (m.param_index, m.param_count, m.param_id, m.param_value)) if m.param_index >= m.param_count: raise ValueError("parameter index (%u) gte parameter count (%u)" % From d1b0072cd82da2052ec716f259ae3e08e3f5423d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 07:23:24 +1100 Subject: [PATCH 5/6] AP_Param: correct maximum-length parameter sanity check need to take into account addition of (eg.) _X suffix for VECTOR3F parameters --- libraries/AP_Param/AP_Param.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 2249d98b0d564..9962780398a66 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -268,7 +268,12 @@ void AP_Param::check_group_info(const struct AP_Param::GroupInfo * group_info, if (size == 0) { FATAL("invalid type in %s", group_info[i].name); } - if (prefix_length + strlen(group_info[i].name) > 16) { + uint8_t param_name_length = prefix_length + strlen(group_info[i].name); + if (type == AP_PARAM_VECTOR3F) { + // need room for _X/_Y/_Z + param_name_length += 2; + } + if (param_name_length > 16) { FATAL("suffix is too long in %s", group_info[i].name); } (*total_size) += size + sizeof(struct Param_header); From 139cbeb15e81aae538d3502081f1d9254e725134 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 07:30:18 +1100 Subject: [PATCH 6/6] AP_Scripting: adjust for renaming of SIM_GPS_DISABLE to SIM_GPS1_ENABLE --- libraries/AP_Scripting/applets/copter-deadreckon-home.lua | 4 ++-- libraries/AP_Scripting/applets/copter-deadreckon-home.md | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Scripting/applets/copter-deadreckon-home.lua b/libraries/AP_Scripting/applets/copter-deadreckon-home.lua index 152b752a0040c..31d5f37785908 100644 --- a/libraries/AP_Scripting/applets/copter-deadreckon-home.lua +++ b/libraries/AP_Scripting/applets/copter-deadreckon-home.lua @@ -31,8 +31,8 @@ -- -- Testing in SITL: -- a. set map setshowsimpos 1 (to allow seeing where vehicle really is in simulator even with GPS disabled) --- b. set SIM_GPS_DISABLE = 1 to disable GPS (confirm dead reckoning begins) --- c. set SIM_GPS_DISABLE = 0 to re-enable GPS +-- b. set SIM_GPS1_ENABLE = 0 to disable GPS (confirm dead reckoning begins) +-- c. set SIM_GPS1_ENABLE = 1 to re-enable GPS -- d. set SIM_GPS_NUMSAT = 3 to lower simulated satellite count to confirm script triggers -- e. set DR_GPS_SACC_MAX = 0.01 to lower the threshold and trigger below the simulator value which is 0.04 (remember to set this back after testing!) -- diff --git a/libraries/AP_Scripting/applets/copter-deadreckon-home.md b/libraries/AP_Scripting/applets/copter-deadreckon-home.md index c45ae3080ccd2..b750eb4d2bf89 100644 --- a/libraries/AP_Scripting/applets/copter-deadreckon-home.md +++ b/libraries/AP_Scripting/applets/copter-deadreckon-home.md @@ -39,8 +39,8 @@ Deadreckoning will only be activated while the vehicle is in autonomous modes (e ## Testing in SITL - set map setshowsimpos 1 (to allow seeing where vehicle really is in simulator even with GPS disabled) - - set SIM_GPS_DISABLE = 1 to disable GPS (confirm dead reckoning begins) - - set SIM_GPS_DISABLE = 0 to re-enable GPS + - set SIM_GPS1_ENABLE = 0 to disable GPS (confirm dead reckoning begins) + - set SIM_GPS1_ENABLE = 1 to re-enable GPS - set SIM_GPS_NUMSAT = 3 to lower simulated satellite count to confirm script triggers - set DR_GPS_SACC_MAX = 0.01 to lower the threshold and trigger below the simulator value which is 0.04 (remember to set this back after testing!)