Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: account for attitude rate in moving baseline data

Browse Source
This commit is contained in:
Andrew Tridgell 2021-04-15 11:34:14 +10:00 committed by Peter Barker
parent 62e0a89036
commit 2168633372
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -426,7 +426,13 @@ 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;
// project attitude back using gyros to get antenna orientation at time of GPS sample
Vector3f gyro(radians(_sitl->state.rollRate),
radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate));
rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw));
const float lag = (1.0/_sitl->gps_hertz[instance]) * gps_state[instance].delay;
rot.rotate(gyro * (-lag));
rel_antenna_pos = rot * rel_antenna_pos;
relposned.version = 1;
relposned.iTOW = time_week_ms;