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

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 <GeographicLib/Geoid.hpp>
#include <memory> // for std::shared_ptr
std::shared_ptr<GeographicLib::Geoid> egm96_5; // This class loads egm96_5 dataset to RAM, it is about 24 MiB.
egm96_5 = std::make_shared<GeographicLib::Geoid>("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 zerois_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->NEDvelocity = 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 requiredreturn;}} 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 requiredreturn;}}

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

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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/647272.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

Django从入门到精通（三）

Django从入门到精通（三）

目录 七、ORM操作 7.1、表结构 常见字段 参数 示例 7.2、表关系 一对多 多对多 第一种方式 第二种方式 7.3、连接MYSQL 7.4、数据库连接池 7.5、多数据库 读写分离 分库&#xff08;多个app ->多数据库&#xff09; 分库&#xff08;单app&#xff09; 注意…
阅读更多...
狗东云搭建幻兽帕鲁(奶妈级别)

狗东云搭建幻兽帕鲁(奶妈级别)

使用狗东云搭建幻兽帕鲁 同配置狗东云比腾讯云便宜&#xff0c;2核2G服务器仅50元1年&#xff0c;4核8G服务器458元1年&#xff0c;点击链接直达. 进入页面会跳转到注册&#xff0c;先注册账户&#xff0c;注册好后页面跳转&#xff0c;没有跳转点这里&#xff0c;选择页面左侧…
阅读更多...
[AIGC 大数据基础] 浅谈hdfs

[AIGC 大数据基础] 浅谈hdfs

HDFS介绍 什么是HDFS&#xff1f; HDFS&#xff08;Hadoop Distributed File System&#xff09;是Apache Hadoop生态系统的一部分&#xff0c;是一个分布式文件系统。它被设计用于存储和处理大规模数据集&#xff0c;并且能够容错、高可靠和高性能地处理文件。 HDFS是为了支…
阅读更多...
2024转行程序员的请注意：均月薪在40-70k

2024转行程序员的请注意：均月薪在40-70k

前言 2023年&#xff0c;对大多数行业来说都是不太好过的一年。 对程序员来说也是如此&#xff0c;很多粉丝朋友都在说android工作特别难找&#xff0c;一个岗位都是几千份简历........大家心里都是特别的焦虑&#xff0c;本以为2024年就业情况会有好转&#xff0c;但实际上并…
阅读更多...
PHP - Yii2 异步队列

PHP - Yii2 异步队列

1. 前言使用场景 在 PHP Yii2 中&#xff0c;队列是一种特殊的数据结构&#xff0c;用于处理和管理后台任务。队列允许我们将耗时的任务&#xff08;如发送电子邮件、push通知等&#xff09;放入队列中&#xff0c;然后在后台异步执行。这样可以避免在处理大量请求时阻塞主应用…
阅读更多...
[GXYCTF2019]BabySQli1

[GXYCTF2019]BabySQli1

单引号闭合&#xff0c;列数为三列&#xff0c;但是没有期待的1 2 3回显&#xff0c;而是显示wrong pass。 尝试报错注入时发现过滤了圆括号&#xff0c;网上搜索似乎也没找到能绕过使用圆括号的方法&#xff0c;那么按以往爆库爆表爆字段的方法似乎无法使用了 在响应报文找到一…
阅读更多...
ORM-07-querydsl 入门介绍

ORM-07-querydsl 入门介绍

拓展阅读 The jdbc pool for java.(java 手写 jdbc 数据库连接池实现) The simple mybatis.&#xff08;手写简易版 mybatis&#xff09; 1. 介绍 1.1 背景 Querydsl的诞生源于以类型安全的方式维护HQL查询的需求。逐步构建HQL查询需要进行字符串连接&#xff0c;导致代码难以…
阅读更多...
迭代器模式-C#实现

迭代器模式-C#实现

该实例基于WPF实现&#xff0c;直接上代码&#xff0c;下面为三层架构的代码。 目录 一 Model 二 View 三 ViewModel 一 Model using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading.Tasks;namespace 设计模式练…
阅读更多...
32个Java面试必考点-06常用工具集

32个Java面试必考点-06常用工具集

本课时主要介绍常用的工具&#xff0c;将会讲解三个知识点&#xff1a; & JVM 相关工具的作用和适用场景&#xff1b; & Git 常用命令和工作流&#xff1b; & Linux 系统中常用分析工具。 常用工具汇总 常用工具汇总如下图所示。 说明&#xff1a;这里列出的都…
阅读更多...
journalctl日期范围操作

journalctl日期范围操作

这里以Docker 日志为例 要查看特定时间段的 Docker 服务日志&#xff0c;你可以使用 journalctl 命令&#xff0c;并结合 -u 选项来指定服务单元名称&#xff08;docker.service&#xff09;以及 -n 和 -S 选项来限制日志的数量和时间范围。 以下是一个示例命令&#xff0c;用…
阅读更多...
k8s的图形化工具--rancher

k8s的图形化工具--rancher

什么是rancher&#xff1f; rancher是一个开源的企业级多集群的k8s管理平台 rancher和k8s的区别 都是为了容器的调度和编排系统&#xff0c;但是rancher不仅能够调度&#xff0c;还能管理k8s集群&#xff0c;自带监控&#xff08;普罗米修斯&#xff09; 实验部署 实验架构…
阅读更多...
电容主要特点和作用，不同类型的电容区别

电容主要特点和作用，不同类型的电容区别

电容 两个相互靠近的金属板中间夹一层绝缘介质组成的器件&#xff0c;当两端存在电势差时&#xff0c;由于介质阻碍了电荷移动而累积在金属板上&#xff0c;衡量金属板上储存电荷的能力称之为电容&#xff0c;相应的器件称为电容器。 电容&#xff08;Capacitance&#xff09…
阅读更多...
移动端 h5-table react版本支持虚拟列表

移动端 h5-table react版本支持虚拟列表

介绍 适用于 react ts 的 h5 移动端项目 table 组件 github 链接 &#xff1a;https://github.com/duKD/react-h5-table 有帮助的话 给个小星星 有两种表格组件 常规的&#xff1a; 支持 左侧固定 滑动 每行点击回调 支持 指定列排序 支持滚动加载更多 效果和之前写的vue…
阅读更多...
【开源】基于JAVA的实验室耗材管理系统

【开源】基于JAVA的实验室耗材管理系统

目录 一、摘要1.1 项目介绍1.2 项目录屏 二、功能模块2.1 耗材档案模块2.2 耗材入库模块2.3 耗材出库模块2.4 耗材申请模块2.5 耗材审核模块 三、系统展示四、核心代码4.1 查询耗材品类4.2 查询资产出库清单4.3 资产出库4.4 查询入库单4.5 资产入库 五、免责说明 一、摘要 1.1…
阅读更多...
四、Kotlin 表达式

四、Kotlin 表达式

1. 常量 & 变量 1.1 可读写变量&#xff08;var&#xff09; var x initValue // x 称为可读写变量注意&#xff1a;当 var 声明的变量做成员属性时&#xff0c;默认提供 setter/getter 方法。 1.2 只读变量&#xff08;val&#xff09; val x initValue // x 称为只…
阅读更多...
FPGA：我的零基础学习路线（2022秋招已上岸）持续更新中~

FPGA：我的零基础学习路线（2022秋招已上岸）持续更新中~

可内推简历&#xff0c;丝我即可 前言 初次接触FPGA是在2022年3月左右&#xff0c;正处在研二下学期&#xff0c;面临着暑假找工作&#xff0c;周围的同学大多选择了互联网&#xff0c;出于对互联网的裁员形势下&#xff0c;我选择了FPGA&#xff0c;对于硬件基础知识我几乎是…
阅读更多...
Vue+OpenLayers7入门到实战：鹰眼控件简单介绍，并使用OpenLayers7在地图上添加鹰眼控件

Vue+OpenLayers7入门到实战：鹰眼控件简单介绍，并使用OpenLayers7在地图上添加鹰眼控件

返回《Vue+OpenLayers7》专栏目录：Vue+OpenLayers7 前言 本章介绍OpenLayers7添加鹰眼控件到地图上的功能。 在OpenLayers中，想要实现鹰眼控件，必须要新建一个数据源，且不能跟其他图层混用，相当于鹰眼是一个单独图层。 补充知识，鹰眼控件是什么？ 鹰眼控件是一种在地…
阅读更多...
windbg 相关命令记录

windbg 相关命令记录

主动下载 pdb symchk.exe /r %windir%\ /s srv*D:\LocalSymbolCache*http://msdl.microsoft.com/download/symbols /od/r是递归&#xff0c;/s是指定存放目录和下载源&#xff0c;/od是输出过程中的详细信息 设置 windows 系统 pdb 下载地址 SRV*C:\LocalSymbolCache*http:/…
阅读更多...
大数据平台红蓝对抗 - 磨利刃，淬精兵！

大数据平台红蓝对抗 - 磨利刃，淬精兵！

背景 目前大促备战常见备战工作&#xff1a;专项压测&#xff08;全链路压测、内部压测&#xff09;、灾备演练、降级演练、限流、巡检&#xff08;监控、应用健康度&#xff09;、混沌演练&#xff08;红蓝对抗&#xff09;&#xff0c;如下图所示。随着平台业务越来越复杂&a…
阅读更多...
Kong工作原理 - 负载均衡 - 基于DNS的负载均衡

Kong工作原理 - 负载均衡 - 基于DNS的负载均衡

Kong提供多种请求负载均衡到多个后端服务的方式&#xff1a;默认的基于DNS的方法&#xff0c;以及使用Upstream实体的一组高级负载均衡算法。 默认情况下启用DNS负载均衡器&#xff0c;仅限于循环调度负载均衡。Upstream实体还具有健康检查和断路器功能&#xff0c;除了更高级…
阅读更多...
最新文章

Copyright @ 2022~2023