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