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

GPS_GLOBAL_ORIGIN: simplify send logic

Browse Source
This commit is contained in:
Matthias Grob
2020-11-19 19:00:37 +01:00
parent 0ca63120eb
commit 65e1b702c9
1 changed files with 6 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp
+6 -9
View File
@@ -85,16 +85,13 @@ protected:
{
vehicle_local_position_s vehicle_local_position;
if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
if (_vehicle_local_position_sub.copy(&vehicle_local_position)
&& vehicle_local_position.xy_global && vehicle_local_position.z_global) {
mavlink_gps_global_origin_t msg{};
if (vehicle_local_position.ref_timestamp > 0) {
msg.latitude = static_cast<int32_t>(vehicle_local_position.ref_lat * 1e7); // double degree -> int32 degreeE7
msg.longitude = static_cast<int32_t>(vehicle_local_position.ref_lon * 1e7); // double degree -> int32 degreeE7
msg.altitude = static_cast<int32_t>(vehicle_local_position.ref_alt * 1e3f); // float m -> int32 mm
msg.time_usec = vehicle_local_position.timestamp; // int64 time since system boot
}
msg.latitude = static_cast<int32_t>(vehicle_local_position.ref_lat * 1e7); // double degree -> int32 degreeE7
msg.longitude = static_cast<int32_t>(vehicle_local_position.ref_lon * 1e7); // double degree -> int32 degreeE7
msg.altitude = static_cast<int32_t>(vehicle_local_position.ref_alt * 1e3f); // float m -> int32 mm
msg.time_usec = vehicle_local_position.timestamp; // int64 time since system boot
mavlink_msg_gps_global_origin_send_struct(_mavlink->get_channel(), &msg);
return true;
}