Skip to content

Commit

Permalink
gps_global_origin: remove LLA to ECEF conversion
Browse files Browse the repository at this point in the history
gps_global_origin is being published as
geographic_msgs::GeoPointStamped
message, which wants LLA format
https://docs.ros.org/en/api/geographic_msgs/html/msg/GeoPointStamped.html

FIX mavlink#1381

Signed-off-by: Beniamino Pozzan <b.pozzan@archangel.im>
  • Loading branch information
beniaminopozzan committed Apr 4, 2024
1 parent 7f1a8c7 commit a996019
Showing 1 changed file with 1 addition and 16 deletions.
17 changes: 1 addition & 16 deletions mavros/src/plugins/global_position.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -233,22 +233,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
g_origin->position.longitude = glob_orig.longitude / 1E7;
g_origin->position.altitude = glob_orig.altitude / 1E3 + m_uas->geoid_to_ellipsoid_height(&g_origin->position); // convert height amsl to height above the ellipsoid

try {
/**
* @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed)
* Note: "earth" frame, in ECEF, of the global origin
*/
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());

earth.Forward(g_origin->position.latitude, g_origin->position.longitude, g_origin->position.altitude,
g_origin->position.latitude, g_origin->position.longitude, g_origin->position.altitude);

gp_global_origin_pub.publish(g_origin);
}
catch (const std::exception& e) {
ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);
}
gp_global_origin_pub.publish(g_origin);
}

/** @todo Handler for GLOBAL_POSITION_INT_COV */
Expand Down

0 comments on commit a996019

Please sign in to comment.