From e8fb082a9a408c79331f0d2761c27a200898e39f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 3 Apr 2020 15:35:19 +1100 Subject: [PATCH] HAL_SITL: fixed simulation of roll/pitch of moving baseline ublox --- libraries/AP_HAL_SITL/sitl_gps.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 2dfdd9bdaa..0f1aacd2ff 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -407,7 +407,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance) const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get(); Vector3f rel_antenna_pos = ant2_pos - ant1_pos; Matrix3f rot; - rot.from_euler(0, 0, radians(d->yaw)); + rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw)); rel_antenna_pos = rot * rel_antenna_pos; relposned.version = 1; relposned.iTOW = time_week_ms;