mavros和PX4中的海拔高与椭球高转换

飞控高度传感器中一般有两种高度：

• 海拔高。也称AMSL（Above Mean Sea Level）height或者geoid height或者正高，顾名思义就是指高于当地平均海平面的高度。我猜气压计测得的高度应当就是与海平面相关的。
• 椭球高。也称ellipsoid height或者大地高。顾名思义就是指相对于WGS84地球标准椭球模型的高度。GPS定位系统普遍采用的WGS84坐标系，给出的高度就是ellipsoid高度。

同一个位置的两种高度值一般可以相差几十米，这是因为当地平均海平面是和地形有关的，凹凸不平没有规律。而ellipsoid height采用的基准是地球标准椭球模型，它的高度值和地形没有关系，只和到地心的距离有关。

下文的mavros指ros1 noetic版本的，ros2版本的我还没研究。

Mavros

mavros/global_position/global话题

此话题发布经纬高，其中高度是ellipsoid height椭球高。其实px4的GLOBAL_POSITION_INT mavlink消息发布的高度是geoid height海拔高度，但是mavros做了转换后再发布到global话题，具体这个转换在：

https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/global_position.cpp#L144-L150

	template
	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
	}

geoid_to_ellipsoid_height()就是海拔高转换为椭球高的函数。要进行这个转换，需要知道地球所有地点的平均海拔高，这是一个相当大的数据集。还记得安装mavros的时候需要执行一个install_geographiclib_datasets.sh脚本吗，其实这就是在安装egm96_5库，这个库存放了地球所有地点的平均海拔高模型，并提供每个经纬度点的海拔高和椭球高的差值。实测导入egm96_5库需要额外的18MB内存。

顺便一提，这个话题发布的经纬度只有七位小数精度，因为PX4的GLOBAL_POSITION_INT mavlink消息stream的时候，将经纬度乘了1e7变成了整型，mavros接收mavlink消息后再除了1e7，因此损失了经纬度七位小数之后的精度（大概相当于几厘米）。

下面这段c++代码可以用于海拔高转换为椭球高：

#include 
#include  // for std::shared_ptr
std::shared_ptr egm96_5; // This class loads egm96_5 dataset to RAM, it is about 24 MiB.
egm96_5 = std::make_shared("egm96-5", "", true, true); // Using smallest dataset with 5' grid, // From default location, // Use cubic interpolation, Thread safe
alt_ellipsoid = alt_amsl + GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lat, lon); // AMSL TO WGS84 altitude

执行代码需要安装egm96，这里借用安装mavros时的脚本：

wget https://gitee.com/shu-peixuan/px4mocap/raw/85b46df9912338f775949903841160c873af4a1d/ROS-install-command/install_geographiclib_datasets.sh
sudo chmod a+x ./install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh # this step takes some time
rm install_geographiclib_datasets.sh

mavros/global_position/local话题

这个话题发布的是ENU东北天坐标。但它和mavros/local_position/pose还不一定一样。它是由ECEF地球坐标系中的坐标相对于地图原点map origin转换到ENU东北天坐标系。这个地图原点的选取可能是当前点，或者地图点，或者设置的map origin。看看mavros源码吧：

https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/global_position.cpp#L330-L365

/**
 * @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();

这里面的odom就是发布到mavros/global_position/local话题的消息。可以看到它的高度默认是相对高度relative_alt。这个相对高度是由px4的GLOBAL_POSITION_INT mavlink消息发布的，是px4飞控内部对于相对地面/起飞点高度的一个估计。

可以看出，mavros/global_position/local话题没什么用，还不如用mavros/local_position/pose。

mavros/setpoint_raw/global话题

这个话题用于向飞控发布期望的经纬高setpoint，我在四旋翼利用mavros进行GPS坐标指点飞行_/mavros/setpoint_raw/local-CSDN博客一文中已经指出，尽量不要直接用这个话题发送GPS目标位置点，可以用mavros/setpoint_raw/local代替。

如果你非要用这个话题，那么我们看看发送的经纬高setpoint中的高度是什么：

https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_raw.cpp#L205-L234

void global_cb(const mavros_msgs::GlobalPositionTarget::ConstPtr &req)
{
	Eigen::Vector3d velocity, af;
	float yaw, yaw_rate;

	tf::vectorMsgToEigen(req->velocity, velocity);
	tf::vectorMsgToEigen(req->acceleration_or_force, af);

	// Transform frame ENU->NED
	velocity = ftf::transform_frame_enu_ned(velocity);
	af = ftf::transform_frame_enu_ned(af);
	yaw = ftf::quaternion_get_yaw(
				ftf::transform_orientation_aircraft_baselink(
					ftf::transform_orientation_ned_enu(
						ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))));
	Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate);
	auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu);
	yaw_rate = ang_vel_ned.z();

	set_position_target_global_int(
				req->header.stamp.toNSec() / 1000000,
				req->coordinate_frame, //代表经纬高采用的坐标系（主要针对高度）
				req->type_mask,
				req->latitude * 1e7,
				req->longitude * 1e7,
				req->altitude, //直接通过SET_POSITION_TARGET_GLOBAL_INT mavlink消息发给px4了
				velocity,
				af,
				yaw, yaw_rate);
}

这里的经纬高是直接通过SET_POSITION_TARGET_GLOBAL_INT mavlink消息发给px4了，经纬度还乘了1e7变成整型，所以损失了第七位小数之后的经纬值（大概相当于几厘米）。

PX4中是怎么处理SET_POSITION_TARGET_GLOBAL_INT mavlink消息的呢，让我们看看px4 v1.13.3（2023年底）中mavlink_receiver.cpp的代码：

https://github.com/PX4/PX4-Autopilot/blob/v1.13.3/src/modules/mavlink/mavlink_receiver.cpp#L1135-L1256

if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
	setpoint.z = local_pos.ref_alt - target_global_int.alt; // setpoint.z就是px4具体track的local高度

} else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
	home_position_s home_position{};
	_home_position_sub.copy(&home_position);

	if (home_position.valid_alt) {
		const float alt = home_position.alt - target_global_int.alt;
		setpoint.z = alt - local_pos.ref_alt; // setpoint.z就是px4具体track的local高度

	} else {
		// home altitude required
		return;
	}

} else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
	vehicle_global_position_s vehicle_global_position{};
	_vehicle_global_position_sub.copy(&vehicle_global_position);

	if (vehicle_global_position.terrain_alt_valid) {
		const float alt = target_global_int.alt + vehicle_global_position.terrain_alt;
		setpoint.z = local_pos.ref_alt - alt; // setpoint.z就是px4具体track的local高度

	} else {
		// valid terrain alt required
		return;
	}

}

原来是和mavlink消息中的coordinate_frame有关，可能是global高度、相对高度或者对地高度。不过这些高度含义其实不那么清晰，具体代表什么还得深挖px4源码。所以再次强调，尽量不要直接用这个话题发送GPS目标位置点。

参考：geoid 理解_egm2008大地水准面模型-CSDN博客