PX4 offboard模式能接收的mavros指令

以下内容针对px4 v1.11.3（2021.01）

px4 offboard模式下可以接收上位机发送来的setpoint值，可以利用ROS包mavros来发送这些setpoint（期望值）。

ps: 如果只是要用上位机发送消息来切换px4模式、解锁等，不需要切换到offboard模式，因为上位机也是利用mavlink给px4发送这些指令，和地面站发送指令是一样的。

本文暂时只讨论mavros中的/mavros/setpoint_raw/local话题，这个话题可以同时发送位置、速度、“加速度”指令，如果要发送姿态角和油门指令还是要用/mavros/setpoint_raw/attitude之类的话题。

那么困扰我很久的问题是：setpoint_raw话题到底能同时发送哪些指令？位置速度能同时发送吗？所谓的加速度指令又是什么呢？

为了解决这些问题，我深入阅读了px4源码中位置环控制程序PositionControl.cpp、offboard模式接收setpoint的程序FlightTaskOffborad.cpp、px4中的飞行模式管理器FlightModeMannager.cpp，px4接受mavlink指令的mavlink_receiver.cpp。px4中类似ros的话题发布方式，FlightModeMannager.cpp中通过选择飞行模式获取mavlink_receiver得到的setpoint通过_trajectory_setpoint_pub.publish(setpoint)发布，MulticopterPositionControl.cpp中通过_trajectory_setpoint_sub.update(&_setpoint)来接受setpint并给PositionControl.cpp中的控制环使用。

总结出了以下规律：

1、如果同时有位置、速度、加速度中至少两项指令给出，那么后者是前馈项。比如同时给出位置和加速度指令，那么加速度指令是前馈项。毕竟px4是位置-速度-加速度-角度-角速度-推力这样的串级控制。位置环是P控制，输出指令速度到速度环，速度环是PID控制，输出指令加速度，加速度再转化为四元数和推力给出角度指令到角度环，角度环是P控制，输出角速度指令到角速度环，角速度环又是PID控制来响应指令角速度。

2、要查看串级控制中每一级的px4响应值，可以在mavros的/mavros/setpoint_raw/target_local话题中查看。可以看到，如果只给位置指令，那么position就是给定的值，velocity和acc虽然没给，但是仍然有变化的值，这是因为他们是上级串级控制的输出。如果只给了速度指令，可以看到position就是NaN，表示没值，velocity是给定值，acc是上级的控制输出。

3、我阅读的是v1.11.3版本的代码，但最近发现在2021.04最新代码v1.12.0beta3中对offboard模式进行了大修，删除了FlightTaskOffborad.cpp和flight_mode_managers/tasks中的offboard模式（改为如果是offboard模式，直接在mavlink_receiver.cpp中用_trajectory_setpoint_pub.publish(setpoint)发布setpoint，绕开了FlightModeManager，px4的github中的PR是#16869）。同时#16869还规范了mavlink_receiver.cpp中接受setpoint的掩码，谁被掩谁就是NaN，谁没被掩谁就被px4接受为setpoint。v1.12.03-beta3中mavlink_receiver.cpp代码如下：

用&按位与来读取type_mask掩码，然后如果是offboard模式，直接通过_trajectory_setpoint_pub发送setpoint给MulticopterPositionControl.cpp，绕过了FlightModeMannager.cpp。不再是mavlink_receiver先发给FlightTaskOffborad.cpp，再由FlightModeMannager.cpp通过_trajectory_setpoint_pub发布给MulticopterPositionControl.cpp。

但就v1.11.3来说，掩码的使用还并不规范，并不是掩了就一定不接收，不掩就一定被接收，整个offboard下的接收setpoint是比较混乱的。可行的组合如下：

• 单独的p、v、acc是没问题的，但是要求x、y、z方向的同时给出。acc是转化为了四元数和归一化油门（读PositionControl.cpp，由悬停油门线性化估计）。给p、v时10-20Hz就行，给加速度或者角度时需要50-100Hz，否则不会按照期望的加速度来飞行的！
• p全给+v全给、pz+v全给。全给指x、y、z同时给出。总之v必须全给，如果不想给，就给前馈0。这其实是v1.11.3中mavlink_receiver.cpp和FlightTaskOffborad.cpp中掩码判断不对导致的。
• p全给+a全给、pz+a全给。总之a必须全给，如果不想给，就给前馈0。和v类似。反正高度环和yaw、yaw_rate是单独控制的，可以组合，但是x、y必须同时给出，有时也会影响到z。
• p全给+v全给+a全给。

除了这些组合，其他组合的/mavros/setpoint_raw掩码在v1.11.3都是不行的！典型的错误:pz+vx+vy（少了vz）。话说这些组合完全够用了呀，不需要的前馈就给0就行。

yaw,yaw_rate都是单独给，可給可不给。都不给好像也没事，就是按照当前yaw来控制,但是yaw会变动，不能保证是定值。

在v1.11.3中都试过以上组合，都没问题的，v1.12.0中当然更没有问题。注意都是给的local坐标系下的值，body系下组合可能又不一样了，global系我也没研究。mavros可能也会把body系的值转化到local系下发送给px4，但px4其实本身也能接受body系下的值。