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

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

news 来源：原创 2025/6/10 7:47:12

使用 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传感器异常或者数据跳变导致飞控拒绝融合时可能不会发布此消息）。

相关文章：

  • Sass具有超能力的CSS预处理器
  • 中山大学GaussianFusion：首个将高斯表示引入端到端自动驾驶多传感器融合的新框架
  • 【渲染】Unity-分析URP的延迟渲染-DeferredShading
  • 基于大模型预测原发性急性闭角型青光眼的技术方案研究大纲
  • 【若依】框架项目部署笔记
  • 均衡后的SNRSINR
  • StarRocks 全面向量化执行引擎深度解析
  • 华为云Flexus+DeepSeek征文 | 基于Dify构建具备联网搜索能力的知识库问答助手
  • 解锁Vscode：C/C++环境配置超详细指南
  • CDBench论文精读
  • 【BUG】记STM32F030多通道ADC DMA读取乱序问题
  • 华为网路设备学习-24（路由器OSPF - 特性专题）
  • 六.原型模式
  • leetcode41-缺失的第一个正数
  • PLC入门【5】基本指令3(PLS PLF ZRST)
  • 加密通信 + 行为分析：运营商行业安全防御体系重构
  • uniapp 字符包含的相关方法
  • 关键领域软件测试的突围之路：如何破解安全与效率的平衡难题
  • Vue数据响应式原理解析
  • vue3 定时器-定义全局方法 vue+ts
  • h5页面制作网站免费/免费网络推广网站
  • 石家庄自适应网站建设/高端婚恋网站排名
  • 雨岑信息科技有限公司做企业型网站做的怎么样_公司规模如何/谷歌商店paypal下载官网
  • 长春做网站多少钱/网站建站流程
  • 怎么样才能自己做网站打广告/网站推广优化方案
  • 住房和城乡建设部网站园林一级/优化什么