|
|
|
@ -85,16 +85,13 @@ protected:
@@ -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; |
|
|
|
|
} |
|
|
|
|