Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add ability to simulate more than 2 GPSs

This commit is contained in:
Peter Barker 2024-11-12 08:49:13 +11:00 committed by Peter Barker
parent 4b679dfb1a
commit fab1ef7a87
9 changed files with 330 additions and 306 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
libraries/SITL/SIM_GPS.cpp
View File

@ -30,6 +30,121 @@
#include <GCS_MAVLink/GCS.h>
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 &params = _sitl->gps[instance];
struct GPS_Data d {};
// simulate delayed lock times
bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
bool have_lock = (params.enabled && now_ms >= params.lock_time*1000UL);
// Only let physics run and GPS write at configured GPS rate (default 5Hz).
if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) {
if ((now_ms - last_write_update_ms) < (uint32_t)(1000/params.hertz)) {
// Reading runs every iteration.
// Beware- physics don't update every iteration with this approach.
// Currently, none of the drivers rely on quickly updated physics.
@ -356,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);
}

6
libraries/SITL/SIM_GPS_NMEA.cpp
View File

@ -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,,",

112
libraries/SITL/SIM_GPS_UBLOX.cpp
View File

@ -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; i<ARRAY_SIZE(_sitl->gps); 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));
}

39
libraries/SITL/SIM_GPS_UBLOX.h
View File

@ -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);
};

2
libraries/SITL/SIM_config.h
View File

@ -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

237
libraries/SITL/SITL.cpp
View File

@ -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

44
libraries/SITL/SITL.h
View File

@ -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;
@ -312,6 +296,32 @@ public:
};
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:

7
libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param
View File

@ -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

23
libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param
View File

@ -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