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

HAL_SITL: allow for separate position for 2nd GPS 

makes for better simulation of moving baseline
This commit is contained in:
Andrew Tridgell 2020-01-26 15:17:20 +11:00
parent fb3496b63b
commit dd1426cf73
3 changed files with 117 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -59,8 +59,7 @@ public:
ArduSub
};
int gps_pipe(void);
int gps2_pipe(void);
int gps_pipe(uint8_t index);
ssize_t gps_read(int fd, void *buf, size_t count);
uint16_t pwm_output[SITL_NUM_CHANNELS];
uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
@ -131,7 +130,7 @@ private:
};
#define MAX_GPS_DELAY 100
gps_data _gps_data[MAX_GPS_DELAY];
gps_data _gps_data[2][MAX_GPS_DELAY];
bool _gps_has_basestation_position;
gps_data _gps_basestation_data;

4
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

@ -64,11 +64,11 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
if (strcmp(path, "GPS1") == 0) {
/* gps */
_connected = true;
_fd = _sitlState->gps_pipe();
_fd = _sitlState->gps_pipe(0);
} else if (strcmp(path, "GPS2") == 0) {
/* 2nd gps */
_connected = true;
_fd = _sitlState->gps2_pipe();
_fd = _sitlState->gps_pipe(1);
} else {
/* parse type:args:flags string for path.
For example:

251
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -33,15 +33,15 @@
using namespace HALSITL;
extern const AP_HAL::HAL& hal;
static uint8_t next_gps_index;
static uint8_t gps_delay;
// state of GPS emulation
static struct gps_state {
/* pipe emulating UBLOX GPS serial stream */
int gps_fd, client_fd;
uint32_t last_update; // milliseconds
} gps_state, gps2_state;
uint8_t next_index;
uint8_t delay;
} gps_state[2];
/*
hook for reading from the GPS pipe
@ -65,39 +65,21 @@ ssize_t SITL_State::gps_read(int fd, void *buf, size_t count)
/*
setup GPS input pipe
*/
int SITL_State::gps_pipe(void)
int SITL_State::gps_pipe(uint8_t idx)
{
int fd[2];
if (gps_state.client_fd != 0) {
return gps_state.client_fd;
if (gps_state[idx].client_fd != 0) {
return gps_state[idx].client_fd;
}
pipe(fd);
gps_state.gps_fd = fd[1];
gps_state.client_fd = fd[0];
gps_state.last_update = AP_HAL::millis();
gps_state[idx].gps_fd = fd[1];
gps_state[idx].client_fd = fd[0];
gps_state[idx].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(gps_state[idx].gps_fd);
HALSITL::UARTDriver::_set_nonblocking(fd[0]);
return gps_state.client_fd;
}
/*
setup GPS2 input pipe
*/
int SITL_State::gps2_pipe(void)
{
int fd[2];
if (gps2_state.client_fd != 0) {
return gps2_state.client_fd;
}
pipe(fd);
gps2_state.gps_fd = fd[1];
gps2_state.client_fd = fd[0];
gps2_state.last_update = AP_HAL::millis();
HALSITL::UARTDriver::_set_nonblocking(gps2_state.gps_fd);
HALSITL::UARTDriver::_set_nonblocking(fd[0]);
return gps2_state.client_fd;
return gps_state[idx].client_fd;
}
/*
@ -105,6 +87,9 @@ int SITL_State::gps2_pipe(void)
*/
void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance)
{
if (instance == 1 && !_sitl->gps2_enable) {
return;
}
while (size--) {
if (_sitl->gps_byteloss > 0.0f) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
@ -114,13 +99,8 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance)
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);
}
if (gps_state[instance].gps_fd != 0) {
write(gps_state[instance].gps_fd, p, 1);
}
p++;
}
@ -1168,7 +1148,6 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD,
double yaw, bool have_lock)
{
struct gps_data d;
char c;
// simulate delayed lock times
@ -1192,111 +1171,107 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
}
}
// run at configured GPS rate (default 5Hz)
if ((AP_HAL::millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) {
return;
}
for (uint8_t idx=0; idx<2; idx++) {
struct gps_data d;
// 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;
d.yaw = yaw;
// 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 (idx == 1 && !_sitl->gps2_enable) {
continue;
}
}
if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) {
return;
}
// Creating GPS2 data by coping GPS data
gps_data d2 = d;
// run at configured GPS rate (default 5Hz)
if ((AP_HAL::millis() - gps_state[idx].last_update) < (uint32_t)(1000/_sitl->gps_hertz)) {
continue;
}
// 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;
// swallow any config bytes
if (gps_state[idx].gps_fd != 0) {
read(gps_state[idx].gps_fd, &c, 1);
}
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);
gps_state[idx].last_update = AP_HAL::millis();
d.latitude = latitude;
d.longitude = longitude;
d.yaw = yaw;
// 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;
if (_sitl->gps_drift_alt > 0) {
// slow altitude drift
d.altitude += _sitl->gps_drift_alt*sinf(AP_HAL::millis()*0.001f*0.02f);
}
// 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[idx];
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;
}
// add in some GPS lag
uint8_t &next_index = gps_state[idx].next_index;
uint8_t &delay = gps_state[idx].delay;
_gps_data[idx][next_index++] = d;
if (next_index >= delay+1) {
next_index = 0;
}
d = _gps_data[idx][next_index];
if (_sitl->gps_delay != delay) {
// cope with updates to the delay control
delay = _sitl->gps_delay;
for (uint8_t i=0; i<delay; i++) {
_gps_data[idx][i] = d;
}
}
if (gps_state[idx].gps_fd == 0) {
continue;
}
// Applying GPS glitch
// Using first gps glitch
Vector3f glitch_offsets = _sitl->gps_glitch[idx];
d.latitude += glitch_offsets.x;
d.longitude += glitch_offsets.y;
d.altitude += glitch_offsets.z;
if (gps_state[idx].gps_fd != 0) {
_update_gps_instance((SITL::SITL::GPSType)_sitl->gps_type[idx].get(), &d, idx);
}
}
}