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

使用 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_usec	uint64_t	us			Timestamp (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_type	uint8_t			GPS_FIX_TYPE	GPS fix type.
lat	int32_t	degE7			Latitude (WGS84, EGM96 ellipsoid)
lon	int32_t	degE7			Longitude (WGS84, EGM96 ellipsoid)
alt	int32_t	毫米			Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude.
eph	uint16_t		1E-2	invalid:UINT16_MAX	GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX
epv	uint16_t		1E-2	invalid:UINT16_MAX	GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX
vel	uint16_t	厘米/秒		invalid:UINT16_MAX	GPS ground speed. If unknown, set to: UINT16_MAX
cog	uint16_t	cdeg		invalid:UINT16_MAX	Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0…359.99 degrees. If unknown, set to: UINT16_MAX
satellites_visible	uint8_t			invalid: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_t	degE5			Heading / track uncertainty
yaw ++	uint16_t	cdeg		invalid:0	Yaw 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传感器异常或者数据跳变导致飞控拒绝融合时可能不会发布此消息）。