当前位置： 首页 > news >正文

【PX4飞控】mavros gps相关话题分析，经纬度海拔获取方法，卫星数锁定状态获取方法

news 2025/6/10 10:17:06

使用 ROS1-Noetic 和 mavros v1.20.1，

携带经纬度海拔的话题主要有三个：

  • /mavros/global_position/raw/fix
  • /mavros/gpsstatus/gps1/raw
  • /mavros/global_position/global

查看 mavros 源码，来分析他们的发布过程。发现前两个话题都对应了同一个 mavlink 消息，他们都在 GPS_RAW_INT 的订阅回调中发布，但是对应不同的源文件，对信息的处理方法略有不同。

/mavros/global_position/raw/fix 的发布在源文件 mavros/mavros/src/plugins/global_position.cpp 中

raw_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("raw/fix", 10);

/mavros/gpsstatus/gps1/raw 的发布在插件中 mavros/mavros_extras/src/plugins/gps_status.cpp 中

gps1_raw_pub = gpsstatus_nh.advertise<mavros_msgs::GPSRAW>("gps1/raw", 10);

搜索两个发布者被调用的位置。

raw_fix_pub 主要用来将原始 GPS 数据（未经 EKF 融合）转发到 /mavros/global_position/raw/fix，并对海拔进行了转换，符合 WGS-84。

// mavros/mavros/src/plugins/global_position.cppvoid handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &raw_gps)
{auto fix = boost::make_shared<sensor_msgs::NavSatFix>();fix->header = m_uas->synchronized_header(child_frame_id, raw_gps.time_usec);fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;if (raw_gps.fix_type > 2)fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;else {ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;}fill_lla(raw_gps, fix);float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;ftf::EigenMapCovariance3d gps_cov(fix->position_covariance.data());// With mavlink v2.0 use accuracies reported by sensorif (msg->magic == MAVLINK_STX &&raw_gps.h_acc > 0 && raw_gps.v_acc > 0) {gps_cov.diagonal() << std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.v_acc / 1E3, 2);fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;}// With mavlink v1.0 approximate accuracies by DOPelse if (!std::isnan(eph) && !std::isnan(epv)) {gps_cov.diagonal() << std::pow(eph * gps_uere, 2), std::pow(eph * gps_uere, 2), std::pow(epv * gps_uere, 2);fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;}else {fill_unknown_cov(fix);}// store & publishm_uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);raw_fix_pub.publish(fix);if (raw_gps.vel != UINT16_MAX &&raw_gps.cog != UINT16_MAX) {double speed = raw_gps.vel / 1E2;				// m/sdouble course = angles::from_degrees(raw_gps.cog / 1E2);	// radauto vel = boost::make_shared<geometry_msgs::TwistStamped>();vel->header.stamp = fix->header.stamp;vel->header.frame_id = frame_id;// From nmea_navsat_drivervel->twist.linear.x = speed * std::sin(course);vel->twist.linear.y = speed * std::cos(course);raw_vel_pub.publish(vel);}// publish satellite countauto sat_cnt = boost::make_shared<std_msgs::UInt32>();sat_cnt->data = raw_gps.satellites_visible;raw_sat_pub.publish(sat_cnt);
}// 涉及子函数
void UAS::update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix,float eph, float epv,int fix_type, int satellites_visible)
{lock_guard lock(mutex);gps_fix = fix;gps_eph = eph;gps_epv = epv;gps_fix_type = fix_type;gps_satellites_visible = satellites_visible;
}

mavlink 消息定义 https://mavlink.io/zh/messages/common.html#GPS_RAW_INT。

注意这个原始消息携带的信息很多，被拆分转发到了多个 ROS 话题中。

time_usecuint64_tusTimestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
fix_typeuint8_tGPS_FIX_TYPEGPS fix type.
latint32_tdegE7Latitude (WGS84, EGM96 ellipsoid)
lonint32_tdegE7Longitude (WGS84, EGM96 ellipsoid)
altint32_t毫米Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude.
ephuint16_t1E-2invalid:UINT16_MAXGPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX
epvuint16_t1E-2invalid:UINT16_MAXGPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX
veluint16_t厘米/秒invalid:UINT16_MAXGPS ground speed. If unknown, set to: UINT16_MAX
coguint16_tcdeginvalid:UINT16_MAXCourse over ground (NOT heading, but direction of movement) in degrees * 100, 0.0…359.99 degrees. If unknown, set to: UINT16_MAX
satellites_visibleuint8_tinvalid:UINT8_MAX可见卫星数量。 If unknown, set to UINT8_MAX
alt_ellipsoid ++int32_t毫米Altitude (above WGS84, EGM96 ellipsoid). Positive for up.
h_acc ++uint32_t毫米Position uncertainty.
v_acc ++uint32_t毫米Altitude uncertainty.
vel_acc ++uint32_t毫米/秒Speed uncertainty.
hdg_acc ++uint32_tdegE5Heading / track uncertainty
yaw ++uint16_tcdeginvalid:0Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.

/mavros/global_position/raw/fix，消息类型是 sensor_msgs/NavSatFix，定义如下

rosmsg show sensor\_msgs/NavSatFixuint8 COVARIANCE_TYPE_UNKNOWN=0
uint8 COVARIANCE_TYPE_APPROXIMATED=1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
uint8 COVARIANCE_TYPE_KNOWN=3
std_msgs/Header headeruint32 seqtime stampstring frame_id
sensor_msgs/NavSatStatus statusint8 STATUS_NO_FIX=-1int8 STATUS_FIX=0int8 STATUS_SBAS_FIX=1int8 STATUS_GBAS_FIX=2uint16 SERVICE_GPS=1uint16 SERVICE_GLONASS=2uint16 SERVICE_COMPASS=4uint16 SERVICE_GALILEO=8int8 statusuint16 service
float64 latitude
float64 longitude
float64 altitude
float64[9] position_covariance
uint8 position_covariance_type

其中 float64 latitude，float64 longitude，float64 altitude 三个字段的赋值过程如下

fill_lla(raw_gps, fix);template<typename MsgT>
inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix)
{fix->latitude = msg.lat / 1E7;		// degfix->longitude = msg.lon / 1E7;		// degfix->altitude = msg.alt / 1E3 + m_uas->geoid_to_ellipsoid_height(fix);	// in meters
}/*** @brief Conversion from height above geoid (AMSL)* to height above ellipsoid (WGS-84)*/
template <class T>
inline double geoid_to_ellipsoid_height(T lla)
{if (egm96_5)return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude);elsereturn 0.0;
}

这里看到经纬度由原始的整型转换到了常用的 degree，且海拔由 mavros 进行了一次转换

  • AMSL（Above Mean Sea Level）：即 “海平面高度”，是 GPS 等设备通常输出的高度。
  • Ellipsoid Height：是相对于地球椭球体（如 WGS-84 椭球）的高度，是 GNSS 内部用来计算的位置。
  • 它们之间的差值由地球重力模型（如EGM96）提供，称为大地水准面起伏（geoid undulation）。

总之输出的 ROS 话题符合 WGS-84。

此外，原始 mavlink 消息包含了两个非常重要的信息，当前GPS锁定状态（QGC 中的 GPS Lock）和接收的卫星数(QGC 的 GPS Count)。他们对室外实物飞行有着重要意义，作为传感器与飞控融合算法状态是否良好的判断依据。

请添加图片描述

当前接收的卫星数量被转发到了另外的话题 /mavros/global_position/raw/satellites。

sat_cnt->data = raw_gps.satellites_visible;
raw_sat_pub.publish(sat_cnt);// 发布者定义如下
// raw_sat_pub = gp_nh.advertise<std_msgs::UInt32>("raw/satellites", 10);

当前GPS锁定状态，发布到了 /mavros/gpsstatus/gps1/raw。这个话题侧重对 GPS_RAW_INT 类型 mavlink 消息进行直接转发，不做任何处理。因此，是 /mavros/global_position/raw/satellites 的超集，和 /mavros/global_position/raw/fix 相比，同样是转发原始的未 ekf 融合的 GPS 数据，/mavros/gpsstatus/gps1/raw 没有对高度进行转发。

/* -*- callbacks -*- */
/*** @brief Publish <a href="https://mavlink.io/en/messages/common.html#GPS_RAW_INT">mavlink GPS_RAW_INT message</a> into the gps1/raw topic.*/
void handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &mav_msg) {auto ros_msg = boost::make_shared<mavros_msgs::GPSRAW>();ros_msg->header            = m_uas->synchronized_header("/wgs84", mav_msg.time_usec);ros_msg->fix_type          = mav_msg.fix_type;ros_msg->lat               = mav_msg.lat;ros_msg->lon               = mav_msg.lon;ros_msg->alt               = mav_msg.alt;ros_msg->eph               = mav_msg.eph;ros_msg->epv               = mav_msg.epv;ros_msg->vel               = mav_msg.vel;ros_msg->cog               = mav_msg.cog;ros_msg->satellites_visible = mav_msg.satellites_visible;ros_msg->alt_ellipsoid     = mav_msg.alt_ellipsoid;ros_msg->h_acc             = mav_msg.h_acc;ros_msg->v_acc             = mav_msg.v_acc;ros_msg->vel_acc           = mav_msg.vel_acc;ros_msg->hdg_acc           = mav_msg.hdg_acc;ros_msg->dgps_numch        = UINT8_MAX;	// information not available in GPS_RAW_INT mavlink messageros_msg->dgps_age          = UINT32_MAX;// information not available in GPS_RAW_INT mavlink messageros_msg->yaw               = mav_msg.yaw;gps1_raw_pub.publish(ros_msg);
}

/mavros/global_position/global 的发布在源文件 mavros/mavros/src/plugins/global_position.cpp 中

// mavros/mavros/src/plugins/global_position.cpp/** @todo Handler for GLOBAL_POSITION_INT_COV */void handle_global_position_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GLOBAL_POSITION_INT &gpos)
{auto odom = boost::make_shared<nav_msgs::Odometry>();auto fix = boost::make_shared<sensor_msgs::NavSatFix>();auto relative_alt = boost::make_shared<std_msgs::Float64>();auto compass_heading = boost::make_shared<std_msgs::Float64>();auto header = m_uas->synchronized_header(child_frame_id, gpos.time_boot_ms);// Global position fixfix->header = header;fill_lla(gpos, fix);// fill GPS status fields using GPS_RAW dataauto raw_fix = m_uas->get_gps_fix();if (raw_fix) {fix->status.service = raw_fix->status.service;fix->status.status = raw_fix->status.status;fix->position_covariance = raw_fix->position_covariance;fix->position_covariance_type = raw_fix->position_covariance_type;}else {// no GPS_RAW_INT -> fix status unknownfix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;// we don't know covariancefill_unknown_cov(fix);}relative_alt->data = gpos.relative_alt / 1E3;	// in meterscompass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN;	// in degrees/*** @brief Global position odometry:** X: spherical coordinate X-axis (meters)* Y: spherical coordinate Y-axis (meters)* Z: spherical coordinate Z-axis (meters)* VX: latitude vel (m/s)* VY: longitude vel (m/s)* VZ: altitude vel (m/s)* Angular rates: unknown* Pose covariance: computed, with fixed diagonal* Velocity covariance: unknown*/odom->header.stamp = header.stamp;odom->header.frame_id = frame_id;odom->child_frame_id = child_frame_id;// Linear velocitytf::vectorEigenToMsg(Eigen::Vector3d(gpos.vy, gpos.vx, gpos.vz) / 1E2,odom->twist.twist.linear);// Velocity covariance unknownftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());vel_cov_out.fill(0.0);vel_cov_out(0) = -1.0;// Current fix in ECEFEigen::Vector3d map_point;try {/*** @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed)** Note: "ecef_origin" is the origin of "map" frame, in ECEF, and the local coordinates are* in spherical coordinates, with the orientation in ENU (just like what is applied* on Gazebo)*/GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(),GeographicLib::Constants::WGS84_f());/*** @brief Checks if the "map" origin is set.* - If not, and the home position is also not received, it sets the current fix as the origin;* - If the home position is received, it sets the "map" origin;* - If the "map" origin is set, then it applies the rotations to the offset between the origin* and the current local geocentric coordinates.*/// Current fix to ECEFmap.Forward(fix->latitude, fix->longitude, fix->altitude,map_point.x(), map_point.y(), map_point.z());// Set the current fix as the "map" origin if it's not setif (!is_map_init && fix->status.status >= sensor_msgs::NavSatStatus::STATUS_FIX) {map_origin.x() = fix->latitude;map_origin.y() = fix->longitude;map_origin.z() = fix->altitude;ecef_origin = map_point; // Local position is zerois_map_init = true;}}catch (const std::exception& e) {ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);}// Compute the local coordinates in ECEFlocal_ecef = map_point - ecef_origin;// Compute the local coordinates in ENUtf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), odom->pose.pose.position);/*** @brief By default, we are using the relative altitude instead of the geocentric* altitude, which is relative to the WGS-84 ellipsoid*/if (use_relative_alt)odom->pose.pose.position.z = relative_alt->data;odom->pose.pose.orientation = m_uas->get_attitude_orientation_enu();// Use ENU covariance to build XYZRPY covarianceftf::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());ftf::EigenMapCovariance6d pos_cov_out(odom->pose.covariance.data());pos_cov_out.setZero();pos_cov_out.block<3, 3>(0, 0) = gps_cov;pos_cov_out.block<3, 3>(3, 3).diagonal() <<rot_cov,rot_cov,rot_cov;// publishgp_fix_pub.publish(fix);gp_odom_pub.publish(odom);gp_rel_alt_pub.publish(relative_alt);gp_hdg_pub.publish(compass_heading);// TFif (tf_send) {geometry_msgs::TransformStamped transform;transform.header.stamp = odom->header.stamp;transform.header.frame_id = tf_frame_id;transform.child_frame_id = tf_child_frame_id;// setRotation()transform.transform.rotation = odom->pose.pose.orientation;// setOrigin()transform.transform.translation.x = odom->pose.pose.position.x;transform.transform.translation.y = odom->pose.pose.position.y;transform.transform.translation.z = odom->pose.pose.position.z;m_uas->tf2_broadcaster.sendTransform(transform);}
}

同样是通过 fill_lla 赋值，发布过程和 /mavros/global_position/raw/fix 类似，对海拔做了转换。

fill_lla(gpos, fix);template<typename MsgT>
inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix)
{fix->latitude = msg.lat / 1E7;		// degfix->longitude = msg.lon / 1E7;		// degfix->altitude = msg.alt / 1E3 + m_uas->geoid_to_ellipsoid_height(fix);	// in meters
}

总结一下：

  • 卫星数和GPS锁定状态可以通过 /mavros/gpsstatus/gps1/raw获取；
  • 未经PX4融合估计的原始的经纬度海拔通过 /mavros/global_position/raw/fix （WGS-84）获取；
  • EKF 融合后的经纬度海拔通过 /mavros/global_position/global
    获取（这个话题的频率实验会比仿真低很多。极端情况，如室内无卫星/GPS传感器异常或者数据跳变导致飞控拒绝融合时可能不会发布此消息）。
http://www.xdnf.cn/news/958375.html

相关文章：

  • StarRocks 全面向量化执行引擎深度解析
  • Spring AI Chat Memory 实战指南：Local 与 JDBC 存储集成
  • YoloV8改进策略：Block改进|FCM，特征互补映射模块|AAAI 2025|即插即用
  • 【三方库研读】facebook/folly中File类原理与作用深度解析
  • PydanticAI快速入门示例
  • JS手写代码篇----使用Promise封装AJAX请求
  • 内网im，局域网环境下BeeWorks 如何保障数据安全？
  • MySQL 隔离级别：脏读、幻读及不可重复读的原理与示例
  • 基于ASP.NET+ SQL Server实现（Web）医院信息管理系统
  • GraphRAG优化新思路-开源的ROGRAG框架
  • python训练营打卡第49天
  • 三元组 题解
  • 日志的具体使用
  • deepseek+coze开发的智能体页面
  • 链表的实现与介绍
  • codeforces C. Cool Partition
  • X86架构离线环境安装Ollama
  • DPC密度峰值聚类
  • 【MPC-C++】qpOASES 源码编译与链接，编译器设置细节
  • bond配置与拆卸
  • 理解OpenFOAM案例中的blockMesh文件里的simpleGrading
  • 【AI论文】CASS：Nvidia到AMD的数据、模型和基准测试的转换
  • 应对无法定位程序输入点kernel32.dll错误的详尽指南：从问题分析到解决方案
  • 如何迁移Cordova应用到HarmonyOS 5 以及迁移时常见的问题？
  • RK3288项目(四)--linux内核之V4L2框架及ov9281驱动分析(中)
  • 深入理解Optional：处理空指针异常
  • upload-labs文件上传_文件上传漏洞_靶场
  • 【MATLAB代码】基于最大相关熵准则（MCC）的三维鲁棒卡尔曼滤波算法（MCC-KF），附源代码|订阅专栏后可直接查看
  • 易学探索助手-个人记录（十四）
  • 【使用LLM搭建系统】5 处理输入: 链式 Prompt Chaining Prompts