Browse Source

HAL_SITL: fixed simulation of roll/pitch of moving baseline ublox

zr-v5.1
Andrew Tridgell 5 years ago
parent
commit
e8fb082a9a
  1. 2
      libraries/AP_HAL_SITL/sitl_gps.cpp

2
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) @@ -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;

Loading…
Cancel
Save