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

Revert "mavlink: GLOBAL_POSITION_INT send without lat/lon availability"

Browse Source 
This reverts commit ad14796b5feb556aa9da8895e12b62b96105a31c.
This commit is contained in:
Lorenz Meier 2020-07-22 10:15:06 +02:00 committed by Daniel Agar
parent e4f65f81f7
commit 3b1be7dcd3
1 changed files with 17 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
src/modules/mavlink/mavlink_messages.cpp
View File

@ -2424,71 +2424,52 @@ protected:
bool send(const hrt_abstime t) override
{
vehicle_global_position_s gpos;
vehicle_local_position_s lpos;
if (_lpos_sub.update(&lpos)) {
if (_gpos_sub.update(&gpos) && _lpos_sub.update(&lpos)) {
mavlink_global_position_int_t msg{};
// time_boot_ms: Timestamp (time since system boot) in ms.
msg.time_boot_ms = lpos.timestamp / 1000;
// alt: Altitude (MSL) in mm.
if (lpos.z_valid && lpos.z_global) {
msg.alt = (-lpos.z + lpos.ref_alt) * 1000.f;
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_sub.copy(&air_data) && (hrt_elapsed_time(&air_data.timestamp) < 10_s)) {
msg.alt = air_data.baro_alt_meter * 1000.f;
if (air_data.timestamp > 0) {
msg.alt = air_data.baro_alt_meter * 1000.0f;
}
}
// relative_alt: Altitude above ground in mm
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.f;
msg.relative_alt = -(lpos.z - home.z) * 1000.0f;
} else {
msg.relative_alt = msg.alt - (home.alt * 1000.f);
msg.relative_alt = msg.alt - (home.alt * 1000.0f);
}
} else {
if (lpos.z_valid) {
msg.relative_alt = -lpos.z * 1000.f;
msg.relative_alt = -lpos.z * 1000.0f;
}
}
// lat, lon: Latitude, Longitude in degE7
vehicle_global_position_s gpos{};
msg.time_boot_ms = gpos.timestamp / 1000;
msg.lat = gpos.lat * 1e7;
msg.lon = gpos.lon * 1e7;
if (_gpos_sub.copy(&gpos) && (hrt_elapsed_time(&gpos.timestamp) < 10_s)) {
msg.lat = gpos.lat * 1e7;
msg.lon = gpos.lon * 1e7;
}
msg.vx = lpos.vx * 100.0f;
msg.vy = lpos.vy * 100.0f;
msg.vz = lpos.vz * 100.0f;
// vx, vy: Ground X, Y Speed (Latitude, positive north) in cm/s
if (lpos.v_xy_valid) {
msg.vx = lpos.vx * 100.f;
msg.vy = lpos.vy * 100.f;
}
// vz: Ground Z Speed (Altitude, positive down) in cm/s
if (lpos.v_z_valid) {
msg.vz = lpos.vz * 100.f;
}
// hdg: Vehicle heading (yaw angle) in cdeg, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
if (PX4_ISFINITE(lpos.yaw)) {
msg.hdg = math::degrees(wrap_2pi(lpos.yaw)) * 100.f;
} else {
msg.hdg = UINT16_MAX;
}
msg.hdg = math::degrees(wrap_2pi(lpos.yaw)) * 100.0f;
mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);