diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index e4e9821d47..0e6877a555 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -273,11 +273,11 @@ void VectorNav::sensorCallback(VnUartPacket *packet) const double lat = positionEstimatedLla.c[0]; const double lon = positionEstimatedLla.c[1]; - const float alt = positionEstimatedLla.c[2]; + const float alt_ellipsoid = positionEstimatedLla.c[2]; if (!_pos_ref.isInitialized()) { _pos_ref.initReference(lat, lon, time_now_us); - _gps_alt_ref = alt; + _gps_alt_ref = alt_ellipsoid; } const Vector2f pos_ned = _pos_ref.project(lat, lon); @@ -292,7 +292,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) local_position.x = pos_ned(0); local_position.y = pos_ned(1); - local_position.z = -(alt - _gps_alt_ref); + local_position.z = -(alt_ellipsoid - _gps_alt_ref); local_position.vx = velocityEstimatedNed.c[0]; local_position.vy = velocityEstimatedNed.c[1]; @@ -342,8 +342,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet) global_position.timestamp_sample = time_now_us; global_position.lat = lat; global_position.lon = lon; - global_position.alt = alt; - global_position.alt = alt; + global_position.lat_lon_valid = true; + global_position.alt = alt_ellipsoid; // AMSL altitude is not available + global_position.alt_ellipsoid = alt_ellipsoid; + global_position.alt_valid = true; global_position.eph = positionUncertaintyEstimated; global_position.epv = positionUncertaintyEstimated;