四旋翼利用mavros进行GPS坐标指点飞行

先介绍一般px4飞控的xyz坐标指点飞行：

利用mavros的 /mavros/setpoint_raw/local 话题可以发送东北天（ENU）坐标给px4飞控进行指点飞行。ENU坐标原点在起飞点，东向为x坐标，北向为y坐标，天向为z坐标。z坐标高度对应 /mavros/local_position/local 中的z坐标，根据px4设置的不同可能是气压计、高度传感器或者差分GPS高度。

下面介绍如何进行gps坐标指点飞行：

户外我们往往希望给定一个gps坐标让四旋翼飞过去，而不是给定一个相对于起飞点的xyz坐标。这时有两种方案：

方法一、直接发送gps坐标

一种方法是利用mavros的 /mavros/setpoint_raw/global 话题直接发送gps坐标和高度给px4飞控，但是我不建议这么做。首先是因为很多人试过用这个话题但是没有成功，其次这个话题里的z坐标高度含义也不那么清晰，我还没明白这个高度到底是对地高度？gps高度？气压计高度？还是海拔高度？实际使用中也极其不建议直接就发gps坐标进行指点飞行，因为我们对经纬度的大小也没有一个直观的感受，经纬度小数点后6位才大致对应1m，给出的经纬度坐标稍有偏差飞机就不知道会飞向何方了，这不利于安全。

方法二、先将gps坐标转化为ENU坐标

第二种方法是将gps坐标先转化为ENU坐标系下的xyz坐标，然后还是用 /mavros/setpoint_raw/local 话题发送给px4飞控。如果你去阅读px4源码里的mavlink_receiver.cpp代码（这是px4接收mavros等mavlink信息的文件），你会发现即使你是用第一种方案向 /mavros/setpoint_raw/global 话题直接发送gps坐标，px4还是会先将gps坐标转化为相对于起飞点的xyz坐标，然后再和 /mavros/setpoint_raw/local 的坐标指令一样地处理。这个px4源码里用的gps坐标转换为ENU坐标的代码如下：

其中map_projection_project函数就是根据给定的参考点gps坐标ref、目标点gps坐标lat和lon，来计算两点之间的xy坐标差（这个x是东，y是北？）。对于原点在起飞点的ENU坐标系，参考点gps坐标其实就是起飞点的gps坐标啦。

map_projection_reproject则是给定ENU坐标系下的xy坐标差和一个参考点gps坐标ref，计算出目标点的gps坐标lat和lon。

这两个转换函数很简单也很常用，但是转换结果未必十分精准。因为地球毕竟是椭球形，将球形平面的经纬度坐标转换为平面内的笛卡尔坐标xy必然有转换误差（主要来源于对椭球的描述偏差）。由此可见，即使是给px4直接发送gps坐标指点飞行，飞机也不是用gps坐标直接进行位置环反馈控制，因此飞行的gps结果也未必十分精准。

我们还在网上找到了更精准的gps坐标转笛卡尔坐标的代码，它将地球按照维度分为了五段，计算过程更复杂但也更准确，xy输出结果和以上px4内部的转换代码大致相差1%。代码链接：使用时引用头文件，然后直接调用函数。也可以参考uavros_simulation/uavros_wrzf_sitl/src/uav_controller.cpp · Peixuan Shu/UAVros - 码云 - 开源中国 (gitee.com)中的gps_to_local函数写法（注意引用头文件WGS84toCartesian.hpp）

注意px4中ENU坐标系用的原点gps坐标未必是home点，一般ENU的原点是飞控上电稳定后的点，但是飞机可能有被搬动或者多次起降的情况，这时home点会变化但是ENU的原点不会变，而且home点更新很慢也容易出错。因此在读取参考点gps坐标时，最好的方式是等飞控和上位机程序启动稳定后（否则读到的gps坐标是错误的！），同时读取当前gps坐标（/mavros/global_position/global) 和ENU坐标 (/mavros/local_position/pose), 利用gps_to_local函数或者前文提到的map_projection_project 算出目标点和当前点的xy坐标差，加上当前的ENU坐标，就能得到目标点的ENU坐标了！这里也可以参考uavros_simulation/uavros_wrzf_sitl/src/uav_controller.cpp · Peixuan Shu/UAVros - 码云 - 开源中国 (gitee.com)

注意

户外gps指点飞行很危险，再三确认坐标值正确，经纬度和xy前后顺序正确，高度正确，再进行指点飞行。直接发送xy坐标指点飞行时，使用的是px4自带的位置环控制，会有大角度机动出现，px4的位置环PID参数为MPC_X等，最大限速v_max也可以修改（QGroundControl地面站中修改）

而且不建议利用指点飞行起飞。如果直接给定飞机上方一点坐标使其起飞，电机会突然从停转状态达到最大转速，飞机也会噌地一下快速飞起来。建议用px4自带的takeoff模式起飞，它会有一个递增的坡度限幅使得电机转速慢慢增加，飞机也是平稳起飞。如果转速增加太慢，可以修改px4的ramp值（QGC搜索ramp或者takeoff）。如果室内用uwb飞行则不能用takeoff模式，因为它的local坐标值是用uwb位置填充的，起飞点就不是（0，0），（而且没有gps导致home点也不会更新？）飞机自动起飞时会直接侧倾向uwb的零点飞过去。