Skip to content

Commit

Permalink
SITL: add ability to simulate more than 2 GPSs
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Nov 11, 2024
1 parent 19964f0 commit d315398
Show file tree
Hide file tree
Showing 7 changed files with 306 additions and 284 deletions.
148 changes: 127 additions & 21 deletions libraries/SITL/SIM_GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,105 @@

#include <GCS_MAVLink/GCS.h>

namespace SITL {
// user settable parameters for airspeed sensors
const AP_Param::GroupInfo SIM::GPSParms::var_info[] = {
// @Param: DISABLE
// @DisplayName: GPS disable
// @Description: Disables GPS
// @Values: 0:Enable, 1:GPS Disabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, GPSParms, enabled, 0, AP_PARAM_FLAG_ENABLE),
// @Param: LAG_MS
// @DisplayName: GPS Lag
// @Description: GPS lag
// @Units: ms
// @User: Advanced
AP_GROUPINFO("LAG_MS", 2, GPSParms, delay_ms, 100),
// @Param: TYPE
// @DisplayName: GPS type
// @Description: Sets the type of simulation used for GPS
// @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP2, 11:Trimble, 19:MSP
// @User: Advanced
AP_GROUPINFO("TYPE", 3, GPSParms, type, GPS::Type::UBLOX),
// @Param: BYTELOS
// @DisplayName: GPS Byteloss
// @Description: Percent of bytes lost from GPS
// @Units: %
// @User: Advanced
AP_GROUPINFO("BYTELOS", 4, GPSParms, byteloss, 0),
// @Param: NUMSATS
// @DisplayName: GPS Num Satellites
// @Description: Number of satellites GPS has in view
AP_GROUPINFO("NUMSATS", 5, GPSParms, numsats, 10),
// @Param: GLITCH
// @DisplayName: GPS Glitch
// @Description: Glitch offsets of simulated GPS sensor
// @Vector3Parameter: 1
// @User: Advanced
AP_GROUPINFO("GLITCH", 6, GPSParms, glitch, 0),
// @Param: HZ
// @DisplayName: GPS Hz
// @Description: GPS Update rate
// @Units: Hz
AP_GROUPINFO("HZ", 7, GPSParms, hertz, 5),
// @Param: DRFTALT
// @DisplayName: GPS Altitude Drift
// @Description: GPS altitude drift error
// @Units: m
// @User: Advanced
AP_GROUPINFO("DRFTALT", 8, GPSParms, drift_alt, 0),
// @Param: POS
// @DisplayName: GPS Position
// @Description: GPS antenna phase center position relative to the body frame origin
// @Units: m
// @Vector3Parameter: 1
AP_GROUPINFO("POS", 9, GPSParms, pos_offset, 0),
// @Param: NOISE
// @DisplayName: GPS Noise
// @Description: Amplitude of the GPS1 altitude error
// @Units: m
// @User: Advanced
AP_GROUPINFO("NOISE", 10, GPSParms, noise, 0),
// @Param: LCKTIME
// @DisplayName: GPS Lock Time
// @Description: Delay in seconds before GPS1 acquires lock
// @Units: s
// @User: Advanced
AP_GROUPINFO("LCKTIME", 11, GPSParms, lock_time, 0),
// @Param: ALT_OFS
// @DisplayName: GPS Altitude Offset
// @Description: GPS Altitude Error
// @Units: m
AP_GROUPINFO("ALT_OFS", 12, GPSParms, alt_offset, 0),
// @Param: HDG
// @DisplayName: GPS Heading
// @Description: Enable GPS1 output of NMEA heading HDT sentence or UBLOX_RELPOSNED
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("HDG", 13, GPSParms, hdg_enabled, SIM::GPS_HEADING_NONE),
// @Param: ACC
// @DisplayName: GPS Accuracy
// @Description: GPS Accuracy
// @User: Advanced
AP_GROUPINFO("ACC", 14, GPSParms, accuracy, 0.3),
// @Param: VERR
// @DisplayName: GPS Velocity Error
// @Description: GPS Velocity Error Offsets in NED
// @Vector3Parameter: 1
// @User: Advanced
AP_GROUPINFO("VERR", 15, GPSParms, vel_err, 0),
// @Param: JAM
// @DisplayName: GPS jamming enable
// @Description: Enable simulated GPS jamming
// @User: Advanced
// @Values: 0:Disabled, 1:Enabled
AP_GROUPINFO("JAM", 16, GPSParms, jam, 0),

AP_GROUPEND
};
}

// the number of GPS leap seconds - copied from AP_GPS.h
#define GPS_LEAPSECONDS_MILLIS 18000ULL

Expand All @@ -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
Expand Down Expand Up @@ -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)) {
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
Expand All @@ -338,15 +444,15 @@ void GPS::update()
_gps_has_basestation_position = true;
}

const uint8_t idx = instance; // alias to avoid code churn
const auto &params = _sitl->gps[instance];

struct GPS_Data d {};

// simulate delayed lock times
bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
bool have_lock = (params.enabled && now_ms >= params.lock_time*1000UL);

// Only let physics run and GPS write at configured GPS rate (default 5Hz).
if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) {
if ((now_ms - last_write_update_ms) < (uint32_t)(1000/params.hertz)) {
// Reading runs every iteration.
// Beware- physics don't update every iteration with this approach.
// Currently, none of the drivers rely on quickly updated physics.
Expand All @@ -356,37 +462,37 @@ void GPS::update()

last_write_update_ms = now_ms;

d.num_sats = _sitl->gps_numsats[idx];
d.num_sats = params.numsats;
d.latitude = latitude;
d.longitude = longitude;
d.yaw_deg = _sitl->state.yawDeg;
d.roll_deg = _sitl->state.rollDeg;
d.pitch_deg = _sitl->state.pitchDeg;

// add an altitude error controlled by a slow sine wave
d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx];
d.altitude = altitude + params.noise * sinf(now_ms * 0.0005f) + params.alt_offset;

// Add offset to c.g. velocity to get velocity at antenna and add simulated error
Vector3f velErrorNED = _sitl->gps_vel_err[idx];
Vector3f velErrorNED = params.vel_err;
d.speedN = speedN + (velErrorNED.x * rand_float());
d.speedE = speedE + (velErrorNED.y * rand_float());
d.speedD = speedD + (velErrorNED.z * rand_float());

d.have_lock = have_lock;

// fill in accuracies
d.horizontal_acc = _sitl->gps_accuracy[idx];
d.vertical_acc = _sitl->gps_accuracy[idx];
d.speed_acc = _sitl->gps_vel_err[instance].get().xy().length();
d.horizontal_acc = params.accuracy;
d.vertical_acc = params.accuracy;
d.speed_acc = params.vel_err.get().xy().length();

if (_sitl->gps_drift_alt[idx] > 0) {
if (params.drift_alt > 0) {
// add slow altitude drift controlled by a slow sine wave
d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f);
d.altitude += params.drift_alt*sinf(now_ms*0.001f*0.02f);
}

// correct the latitude, longitude, height and NED velocity for the offset between
// the vehicle c.g. and GPS antenna
Vector3f posRelOffsetBF = _sitl->gps_pos_offset[idx];
Vector3f posRelOffsetBF = params.pos_offset;
if (!posRelOffsetBF.is_zero()) {
// get a rotation matrix following DCM conventions (body to earth)
Matrix3f rotmat;
Expand Down Expand Up @@ -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);
}
Expand Down
6 changes: 3 additions & 3 deletions libraries/SITL/SIM_GPS_NMEA.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,,",
Expand Down
112 changes: 52 additions & 60 deletions libraries/SITL/SIM_GPS_UBLOX.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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));
Expand All @@ -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));
}

Expand Down
Loading

0 comments on commit d315398

Please sign in to comment.