The current SITL HAL implementation of the GPS hardware sensor can be found in: https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_HAL_SITL/sitl_gps.cpp
This implementation currently sets up pipe(s) to emulate the hardware I2C gps data in the following code:
/*
setup GPS input pipe
*/
int SITL_State::gps_pipe(void)
{
int fd[2];
if (gps_state.client_fd != 0) {
return gps_state.client_fd;
}
pipe(fd);
gps_state.gps_fd = fd[1];
gps_state.client_fd = fd[0];
gps_state.last_update = AP_HAL::millis();
fcntl(fd[0], F_SETFD, FD_CLOEXEC);
fcntl(fd[1], F_SETFD, FD_CLOEXEC);
HALSITL::UARTDriver::_set_nonblocking(gps_state.gps_fd);
HALSITL::UARTDriver::_set_nonblocking(fd[0]);
return gps_state.client_fd;
}
We can then write data to the GPS using this function:
/*
write some bytes from the simulated GPS
*/
void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance)
{
while (size--) {
if (_sitl->gps_byteloss > 0.0f) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
if (r < _sitl->gps_byteloss) {
// lose the byte
p++;
continue;
}
}
if (instance == 0 && gps_state.gps_fd != 0) {
write(gps_state.gps_fd, p, 1);
}
if (instance == 1 && _sitl->gps2_enable) {
if (gps2_state.gps_fd != 0) {
write(gps2_state.gps_fd, p, 1);
}
}
p++;
}
}
Currently this function takes the current gps state and writes it to the pipe we set up earlier. Logically, we need to find out how this gps_state is updated because it isn't done in the function above.
The function call that is made to update the GPS is here:
/*
possibly send a new GPS packet
*/
void SITL_State::_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD, bool have_lock)
{
struct gps_data d;
char c;
// simulate delayed lock times
if (AP_HAL::millis() < _sitl->gps_lock_time*1000UL) {
have_lock = false;
}
altitude += _sitl->gps_alt_offset;
//Capture current position as basestation location for
if (!_gps_has_basestation_position) {
if (have_lock) {
_gps_basestation_data.latitude = latitude;
_gps_basestation_data.longitude = longitude;
_gps_basestation_data.altitude = altitude;
_gps_basestation_data.speedN = speedN;
_gps_basestation_data.speedE = speedE;
_gps_basestation_data.speedD = speedD;
_gps_basestation_data.have_lock = have_lock;
_gps_has_basestation_position = true;
}
}
// run at configured GPS rate (default 5Hz)
if ((AP_HAL::millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) {
return;
}
// swallow any config bytes
if (gps_state.gps_fd != 0) {
read(gps_state.gps_fd, &c, 1);
}
if (gps2_state.gps_fd != 0) {
read(gps2_state.gps_fd, &c, 1);
}
gps_state.last_update = AP_HAL::millis();
gps2_state.last_update = AP_HAL::millis();
d.latitude = latitude;
d.longitude = longitude;
// add an altitude error controlled by a slow sine wave
d.altitude = altitude + _sitl->gps_noise * sinf(AP_HAL::millis() * 0.0005f);
// Add offet to c.g. velocity to get velocity at antenna
d.speedN = speedN;
d.speedE = speedE;
d.speedD = speedD;
d.have_lock = have_lock;
// correct the latitude, longitude, hiehgt and NED velocity for the offset between
// the vehicle c.g. and GPs antenna
Vector3f posRelOffsetBF = _sitl->gps_pos_offset;
if (!posRelOffsetBF.is_zero()) {
// get a rotation matrix following DCM conventions (body to earth)
Matrix3f rotmat;
_sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the antenna offset into the earth frame
Vector3f posRelOffsetEF = rotmat * posRelOffsetBF;
// Add the offset to the latitude, longitude and height using a spherical earth approximation
double const earth_rad_inv = 1.569612305760477e-7; // use Authalic/Volumetric radius
double lng_scale_factor = earth_rad_inv / cos(radians(d.latitude));
d.latitude += degrees(posRelOffsetEF.x * earth_rad_inv);
d.longitude += degrees(posRelOffsetEF.y * lng_scale_factor);
d.altitude -= posRelOffsetEF.z;
// calculate a velocity offset due to the antenna position offset and body rotation rate
// note: % operator is overloaded for cross product
Vector3f gyro(radians(_sitl->state.rollRate),
radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate));
Vector3f velRelOffsetBF = gyro % posRelOffsetBF;
// rotate the velocity offset into earth frame and add to the c.g. velocity
Vector3f velRelOffsetEF = rotmat * velRelOffsetBF;
d.speedN += velRelOffsetEF.x;
d.speedE += velRelOffsetEF.y;
d.speedD += velRelOffsetEF.z;
}
if (_sitl->gps_drift_alt > 0) {
// slow altitude drift
d.altitude += _sitl->gps_drift_alt*sinf(AP_HAL::millis()*0.001f*0.02f);
}
// add in some GPS lag
_gps_data[next_gps_index++] = d;
if (next_gps_index >= gps_delay+1) {
next_gps_index = 0;
}
d = _gps_data[next_gps_index];
if (_sitl->gps_delay != gps_delay) {
// cope with updates to the delay control
gps_delay = _sitl->gps_delay;
for (uint8_t i=0; i<gps_delay; i++) {
_gps_data[i] = d;
}
}
if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) {
return;
}
// Creating GPS2 data by coping GPS data
gps_data d2 = d;
// Applying GPS glitch
// Using first gps glitch
Vector3f glitch_offsets = _sitl->gps_glitch;
d.latitude += glitch_offsets.x;
d.longitude += glitch_offsets.y;
d.altitude += glitch_offsets.z;
// Using second gps glitch
glitch_offsets = _sitl->gps2_glitch;
d2.latitude += glitch_offsets.x;
d2.longitude += glitch_offsets.y;
d2.altitude += glitch_offsets.z;
if (gps_state.gps_fd != 0) {
_update_gps_instance((SITL::SITL::GPSType)_sitl->gps_type.get(), &d, 0);
}
if (gps2_state.gps_fd != 0) {
_update_gps_instance((SITL::SITL::GPSType)_sitl->gps2_type.get(), &d2, 1);
}
}
If we want to inject false GPS data, my best guess is that we need to modify this function to take our fake data.