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

EKF2: add validity flags to global pos message (#23787)

Browse Source 
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Marco Hauswirth
2024-10-23 10:19:04 +02:00
committed by GitHub
co-authored by Mathieu Bresciani
parent 808153b049
commit 0c451552c7
16 changed files with 87 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp
+4 -22
View File
@@ -72,39 +72,21 @@ private:
mavlink_global_position_int_t msg{};
if (lpos.z_valid && lpos.z_global) {
msg.alt = (-lpos.z + lpos.ref_alt) * 1000.0f;
} else {
// fall back to baro altitude
vehicle_air_data_s air_data{};
_air_data_sub.copy(&air_data);
if (air_data.timestamp > 0) {
msg.alt = air_data.baro_alt_meter * 1000.0f;
}
}
home_position_s home{};
_home_sub.copy(&home);
if ((home.timestamp > 0) && home.valid_alt) {
if (lpos.z_valid) {
msg.relative_alt = -(lpos.z - home.z) * 1000.0f;
} else {
msg.relative_alt = msg.alt - (home.alt * 1000.0f);
}
msg.relative_alt = (gpos.alt - home.alt) * 1000.0f;
} else {
if (lpos.z_valid) {
msg.relative_alt = -lpos.z * 1000.0f;
}
msg.relative_alt = gpos.alt * 1000.0f;
}
msg.time_boot_ms = gpos.timestamp / 1000;
msg.lat = gpos.lat * 1e7;
msg.lon = gpos.lon * 1e7;
msg.alt = gpos.alt * 1000.0f;
msg.vx = lpos.vx * 100.0f;
msg.vy = lpos.vy * 100.0f;