fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

VectorNav: set global position validity

Browse Source
This commit is contained in:
bresch 2025-11-13 10:53:21 +01:00 committed by Mathieu Bresciani
parent 0cefd74fee
commit 45e8712d60
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/ins/vectornav/VectorNav.cpp
View File

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