肃宁做网站,域名注册方法,网站广告设计,微信上做网站怎么做飞控高度传感器中一般有两种高度#xff1a;
海拔高。也称AMSL#xff08;Above Mean Sea Level#xff09;height或者geoid height或者正高#xff0c;顾名思义就是指高于当地平均海平面的高度。我猜气压计测得的高度应当就是与海平面相关的。椭球高。也称ellipsoid heig…飞控高度传感器中一般有两种高度
海拔高。也称AMSLAbove Mean Sea Levelheight或者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 templatetypename MsgTinline 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_ptrGeographicLib::Geoid egm96_5; // This class loads egm96_5 dataset to RAM, it is about 24 MiB.
egm96_5 std::make_sharedGeographicLib::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 ax ./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 its 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.32023年底中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博客
