【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.cpp

void 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 sensor
	if (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 DOP
	else 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 & publish
	m_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/s
		double course = angles::from_degrees(raw_gps.cog / 1E2);	// rad

		auto vel = boost::make_shared<geometry_msgs::TwistStamped>();

		vel->header.stamp = fix->header.stamp;
		vel->header.frame_id = frame_id;

		// From nmea_navsat_driver
		vel->twist.linear.x = speed * std::sin(course);
		vel->twist.linear.y = speed * std::cos(course);

		raw_vel_pub.publish(vel);
	}

	// publish satellite count
	auto 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 话题中。

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

rosmsg show sensor\_msgs/NavSatFix

uint8 COVARIANCE_TYPE_UNKNOWN=0
uint8 COVARIANCE_TYPE_APPROXIMATED=1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
uint8 COVARIANCE_TYPE_KNOWN=3
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
sensor_msgs/NavSatStatus status
  int8 STATUS_NO_FIX=-1
  int8 STATUS_FIX=0
  int8 STATUS_SBAS_FIX=1
  int8 STATUS_GBAS_FIX=2
  uint16 SERVICE_GPS=1
  uint16 SERVICE_GLONASS=2
  uint16 SERVICE_COMPASS=4
  uint16 SERVICE_GALILEO=8
  int8 status
  uint16 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;		// deg
	fix->longitude = msg.lon / 1E7;		// deg
	fix->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);
	else
		return 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 message
	ros_msg->dgps_age          = UINT32_MAX;// information not available in GPS_RAW_INT mavlink message
	ros_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 fix
	fix->header = header;

	fill_lla(gpos, fix);

	// fill GPS status fields using GPS_RAW data
	auto 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 unknown
		fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
		fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;

		// we don't know covariance
		fill_unknown_cov(fix);
	}

	relative_alt->data = gpos.relative_alt / 1E3;	// in meters
	compass_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 velocity
	tf::vectorEigenToMsg(Eigen::Vector3d(gpos.vy, gpos.vx, gpos.vz) / 1E2,
				odom->twist.twist.linear);

	// Velocity covariance unknown
	ftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());
	vel_cov_out.fill(0.0);
	vel_cov_out(0) = -1.0;

	// Current fix in ECEF
	Eigen::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 ECEF
		map.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 set
		if (!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 zero
			is_map_init = true;
		}
	}
	catch (const std::exception& e) {
		ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);
	}

	// Compute the local coordinates in ECEF
	local_ecef = map_point - ecef_origin;
	// Compute the local coordinates in ENU
	tf::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 covariance
	ftf::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;

	// publish
	gp_fix_pub.publish(fix);
	gp_odom_pub.publish(odom);
	gp_rel_alt_pub.publish(relative_alt);
	gp_hdg_pub.publish(compass_heading);

	// TF
	if (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;		// deg
	fix->longitude = msg.lon / 1E7;		// deg
	fix->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传感器异常或者数据跳变导致飞控拒绝融合时可能不会发布此消息）。