aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_SITL: gps correct glitchs

Browse Source
This commit is contained in:
Pierre Kancir 2017-04-10 12:17:09 +02:00 committed by Francisco Ferreira
parent 949aa4d9ca
commit 327ac07d54
1 changed files with 27 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -282,19 +282,11 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
gps_time(&time_week, &time_week_ms); gps_time(&time_week, &time_week_ms);
// add glitch to 2nd gps
Vector3f glitch;
if (instance == 0) {
glitch = _sitl->gps_glitch;
} else {
glitch = _sitl->gps2_glitch;
}
pos.time = time_week_ms; pos.time = time_week_ms;
pos.longitude = (d->longitude + glitch.y) * 1.0e7; pos.longitude = d->longitude * 1.0e7;
pos.latitude = (d->latitude + glitch.x) * 1.0e7; pos.latitude = d->latitude * 1.0e7;
pos.altitude_ellipsoid = (d->altitude + glitch.z) * 1000.0f; pos.altitude_ellipsoid = d->altitude * 1000.0f;
pos.altitude_msl = (d->altitude + glitch.z) * 1000.0f; pos.altitude_msl = d->altitude * 1000.0f;
pos.horizontal_accuracy = 1500; pos.horizontal_accuracy = 1500;
pos.vertical_accuracy = 2000; pos.vertical_accuracy = 2000;
@ -349,10 +341,10 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok
pvt.flags2 =0; pvt.flags2 =0;
pvt.num_sv = d->have_lock?_sitl->gps_numsats:3; pvt.num_sv = d->have_lock?_sitl->gps_numsats:3;
pvt.lon = (d->longitude + glitch.y) * 1.0e7; pvt.lon = d->longitude * 1.0e7;
pvt.lat = (d->latitude + glitch.x) * 1.0e7; pvt.lat = d->latitude * 1.0e7;
pvt.height = (d->altitude + glitch.z) * 1000.0f; pvt.height = d->altitude * 1000.0f;
pvt.h_msl = (d->altitude + glitch.z) * 1000.0f; pvt.h_msl = d->altitude * 1000.0f;
pvt.h_acc = 200; pvt.h_acc = 200;
pvt.v_acc = 200; pvt.v_acc = 200;
pvt.velN = 1000.0f * d->speedN; pvt.velN = 1000.0f * d->speedN;
@ -1119,7 +1111,6 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
{ {
struct gps_data d; struct gps_data d;
char c; char c;
Vector3f glitch_offsets;// = _sitl->gps_glitch;
//Capture current position as basestation location for //Capture current position as basestation location for
if (!_gps_has_basestation_position) { if (!_gps_has_basestation_position) {
@ -1151,10 +1142,10 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
gps_state.last_update = AP_HAL::millis(); gps_state.last_update = AP_HAL::millis();
gps2_state.last_update = AP_HAL::millis(); gps2_state.last_update = AP_HAL::millis();
d.latitude = latitude + glitch_offsets.x; d.latitude = latitude;
d.longitude = longitude + glitch_offsets.y; d.longitude = longitude;
// add an altitude error controlled by a slow sine wave // add an altitude error controlled by a slow sine wave
d.altitude = altitude + glitch_offsets.z + _sitl->gps_noise * sinf(AP_HAL::millis() * 0.0005f); d.altitude = altitude + _sitl->gps_noise * sinf(AP_HAL::millis() * 0.0005f);
// Add offet to c.g. velocity to get velocity at antenna // Add offet to c.g. velocity to get velocity at antenna
d.speedN = speedN; d.speedN = speedN;
@ -1218,6 +1209,20 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) { if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) {
return; 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;
switch ((SITL::SITL::GPSType)_sitl->gps_type.get()) { switch ((SITL::SITL::GPSType)_sitl->gps_type.get()) {
case SITL::SITL::GPS_TYPE_NONE: case SITL::SITL::GPS_TYPE_NONE:
@ -1225,8 +1230,8 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
break; break;
case SITL::SITL::GPS_TYPE_UBLOX: case SITL::SITL::GPS_TYPE_UBLOX:
_update_gps_ubx(&d,0); _update_gps_ubx(&d, 0);
_update_gps_ubx(&d,1); _update_gps_ubx(&d2, 1);
break; break;
case SITL::SITL::GPS_TYPE_MTK: case SITL::SITL::GPS_TYPE_MTK: