From 341b926a430a3fbb5284ac2aac59d14152eb1c46 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 4 Mar 2017 11:39:37 +0900 Subject: [PATCH] AP_HAL_SITL: add glitch for 2nd ublox --- libraries/AP_HAL_SITL/SITL_State.h | 6 ++-- libraries/AP_HAL_SITL/sitl_gps.cpp | 57 +++++++++++++++++------------- 2 files changed, 36 insertions(+), 27 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index d585ee65db..4e8ae809db 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -103,9 +103,9 @@ private: bool _gps_has_basestation_position; gps_data _gps_basestation_data; - void _gps_write(const uint8_t *p, uint16_t size); - void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); - void _update_gps_ubx(const struct gps_data *d); + void _gps_write(const uint8_t *p, uint16_t size, uint8_t instance = 0); + void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size, uint8_t instance); + void _update_gps_ubx(const struct gps_data *d, uint8_t instance); void _update_gps_mtk(const struct gps_data *d); void _update_gps_mtk16(const struct gps_data *d); void _update_gps_mtk19(const struct gps_data *d); diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index aecb91ece0..c70df8e265 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -101,7 +101,7 @@ int SITL_State::gps2_pipe(void) /* write some bytes from the simulated GPS */ -void SITL_State::_gps_write(const uint8_t *p, uint16_t size) +void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance) { while (size--) { if (_sitl->gps_byteloss > 0.0f) { @@ -112,10 +112,10 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size) continue; } } - if (gps_state.gps_fd != 0) { + if (instance == 0 && gps_state.gps_fd != 0) { write(gps_state.gps_fd, p, 1); } - if (_sitl->gps2_enable) { + if (instance == 1 && _sitl->gps2_enable) { if (gps2_state.gps_fd != 0) { write(gps2_state.gps_fd, p, 1); } @@ -146,7 +146,7 @@ static void simulation_timeval(struct timeval *tv) /* send a UBLOX GPS message */ -void SITL_State::_gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) +void SITL_State::_gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size, uint8_t instance) { const uint8_t PREAMBLE1 = 0xb5; const uint8_t PREAMBLE2 = 0x62; @@ -165,9 +165,9 @@ void SITL_State::_gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) for (uint8_t i=0; igps_glitch; + } else { + glitch = _sitl->gps2_glitch; + } + pos.time = time_week_ms; - pos.longitude = d->longitude * 1.0e7; - pos.latitude = d->latitude * 1.0e7; - pos.altitude_ellipsoid = d->altitude*1000.0f; - pos.altitude_msl = d->altitude*1000.0f; + pos.longitude = (d->longitude + glitch.y) * 1.0e7; + pos.latitude = (d->latitude + glitch.x) * 1.0e7; + pos.altitude_ellipsoid = (d->altitude + glitch.z) * 1000.0f; + pos.altitude_msl = (d->altitude + glitch.z) * 1000.0f; pos.horizontal_accuracy = 1500; pos.vertical_accuracy = 2000; @@ -341,10 +349,10 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d) pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok pvt.flags2 =0; pvt.num_sv = d->have_lock?_sitl->gps_numsats:3; - pvt.lon = d->longitude * 1.0e7; - pvt.lat = d->latitude * 1.0e7; - pvt.height = d->altitude*1000.0f; - pvt.h_msl = d->altitude*1000.0f; + pvt.lon = (d->longitude + glitch.y) * 1.0e7; + pvt.lat = (d->latitude + glitch.x) * 1.0e7; + pvt.height = (d->altitude + glitch.z) * 1000.0f; + pvt.h_msl = (d->altitude + glitch.z) * 1000.0f; pvt.h_acc = 200; pvt.v_acc = 200; pvt.velN = 1000.0f * d->speedN; @@ -359,12 +367,12 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d) pvt.headVeh = 0; memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2)); - _gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); - _gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status)); - _gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); - _gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); - _gps_send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop)); - _gps_send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt)); + _gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos), instance); + _gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status), instance); + _gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned), instance); + _gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol), instance); + _gps_send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop), instance); + _gps_send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt), instance); } static void swap_uint32(uint32_t *v, uint8_t n) @@ -984,7 +992,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, { struct gps_data d; char c; - Vector3f glitch_offsets = _sitl->gps_glitch; + Vector3f glitch_offsets;// = _sitl->gps_glitch; //Capture current position as basestation location for if (!_gps_has_basestation_position) { @@ -1092,7 +1100,8 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, break; case SITL::SITL::GPS_TYPE_UBLOX: - _update_gps_ubx(&d); + _update_gps_ubx(&d,0); + _update_gps_ubx(&d,1); break; case SITL::SITL::GPS_TYPE_MTK: