px4+vins+ego单机鲁棒飞行一（px4+mavros篇）

一、mavors安装

参考教程

1. 添加用户权限

sudo usermod -a -G dialout $USER

2. 二进制安装mavros

sudo apt-get install ros-melodic-mavros ros-melodic-mavros-extras ros-melodic-control-toolbox

3. 安装GeographicLib

cd /opt/ros/melodic/lib/mavros
sudo ./install_geographiclib_datasets.sh

4. 测试
   如果是通过Micro USB连接，则：

roslaunch mavros px4.launch fcu_url:="/dev/ttyACM0:921600" gcs_url:=udp-b://@

如果是通过USB转TTL连接，则：

roslaunch mavros px4.launch fcu_url:="/dev/ttyUSB0:921600" gcs_url:=udp-b://@

二、mavros中的重要插件Plugins

参考：、官方wiki、官方wiki扩充、官方msg大全、飞行模式
PS：想要用哪个插件，就必须放在白名单中
2.1 3dr_radio
作用：发布无线电状态，用于APM and PX4的3DR Radio数传
发送topic：/mavros/radio_status
msg信息：mavros_msgs/RadioStatus

2.2 actuator_control
作用：向FCU发送执行器命令
订阅topic：mavros_msgs/ActuatorControl
msg信息：mavros_msgs/ActuatorControl

2.3 vision_pose_estimate
作用：向px4发送视觉里程计，用于位姿融合
订阅topic：/mavros/vision_pose/pose
msg信息：geometry_msgs/PoseStamped

2.4 setpoint_raw
作用：设置目标（视觉估计为local，GPS为global，气压计为attitude）
订阅topic：/mavros/setpoint_raw/local、/mavros/setpoint_raw/global、/mavros/setpoint_raw/attitude
msg信息：mavros_msgs/PositionTarget、mavros_msgs/GlobalPositionTarget、mavros_msgs/AttitudeTarget
注：类似插件有setpoint_accel、setpoint_attitude、setpoint_position、setpoint_velocity
，但我一般只用setpoint_raw插件中的/mavros/setpoint_raw/local，用来设置无人机的目标位置（ego-planner中发送的就是这个对应的消息类型）。

2.5 local_position
作用：Local position from FCU，Velocity data from FCU。
发送topic：/mavros/local_position/pose、/mavros/local_position/velocity
msg信息：geometry_msgs/PoseStamped、geometry_msgs/TwistStamped

2.6 manual_control（user input）
作用：遥控器数值的订阅与发送
订阅topic：/mavros/manual_control/send
发送topic：/mavros/manual_control/control
msg信息：mavros_msgs/ManualControl、mavros_msgs/ManualControl

2.7 rc_io
作用：发布遥控器信息，也可以对遥控器进行override，但是不建议。实在想用override，建议用setpoint plugins and OFFBOARD mode
订阅topic：/mavros/rc/override
发布topic：/mavros/rc/in(Publish RC inputs (in raw milliseconds) )、/mavrosrc/out(Publish FCU servo outputs )
msg信息：mavros_msgs/OverrideRCIn、mavros_msgs/RCIn、mavros_msgs/RCOut

2.8 safety_area
作用：Sends safety allowed area to FCU. Initial area can be loaded from parameters
订阅topic：/mavros/safety_area/set
msg：geometry_msgs/PolygonStamped

2.9 sys_status
作用：System status plugin. Handles FCU detection. REQUIRED never blacklist it!.
发送topic：/mavros/state、/mavros/battery、/mavros/extended_state（Landing detector and VTOL state）
Services：/mavros/set_mode
msg：mavros_msgs/State、mavros_msgs/BatteryStatus、mavros_msgs/ExtendedState、mavros_msgs/SetMode

2.10 command
作用：向PX4飞控发送命令
Services：/mavros/cmd/command、/mavros/cmd/command_int、/mavros/cmd/arming 、/mavros/cmd/set_home、/mavros/cmd/takeoff、/mavros/cmd/land 、/mavros/cmd/trigger_control
msg：略

三、mavros中的重要topic

除上述提到的topic外，还有些重要的topic。
3.1 /mavros/local_position/odom
msg：nav_msgs/Odometry

四、mavros中的tf树

4.1 如果想要将vins中的tf中的world同mavros连起来，需要修改mavros的配置文件px4_config.yaml文件，如果是二进制安装，则在/opt/ros//melodic/share/mavros/launch下的px4_config.yaml中：

local_position:
  frame_id: "world"
  tf:
    send: true
    frame_id: "world"
    child_frame_id: "base_link"
    send_fcu: true

4.2 如果想将bash_link同D435i发布的camera_link连接起来，则需要添加一个静态tf变换的launch文件：

<node pkg="tf" type="static_transform_publisher" name="body_to_base_link" 
    args="0.13 0 0 -1.57 0 -1.57 /base_link /camera_link 30" /> 

因为在x方向摄像头在前方，所以添加一个0.13m的位移变换，又摄像头FRU左边线和车身的ENU坐标系的x,y刚好换了，所以在z轴旋转pi/2，对比vins原始数据，还需要在roll上旋转pi/2。总而言之，是为了另转换后的camer_link和vins的里程计信息一模一样。

五、px4校准

可参考：参考一

1. Airframe
   选择3DR DIY Quad或者DJI F450等相符的。

2. Sensors传感器校准

3. Radio遥控器校准

4. Flight Modes
   

5. Power
   5.1 校正电压计
   5.2 校正电调ESC

6. Motors
   测试电机转向是否正确，如果不正确，任意调换两根三相电源线。如果ID不对，则调整对应的电调数据线。
   

7. Safety
   可视个人情况调整。

六、px4配置

1. 固件
   px4固件1.11.3
   有的固件不会出来world tf树，比如1.12.0
2. 解锁提示“Flying with usb is not safe”
   原因：使用的是micro usb连接
   解决办法：QGC参数表中设置CBRK_USB_CHK=197848
3. 解锁提示press safety switch first
   原因：没有按GPS模块上的安全开关
   解决办法：QGC参数表中设置CBRK_IO_SAFETY=22027
4. 外部位姿估计
5. 设置自动降落模式降落速度
   MPC_LAND_SPEED = 0.3m/s
6. 设置遥控器遥感灵敏度

七、固件编译

1. 下载固件

git clone https://github.com/PX4/PX4-Autopilot.git
cd ~/PX4-Autopilot
git checkout v1.11.3
git submodule update --init --recursive

2. 安装依赖

cd ~/PX4-Autopilot/Tools/setup
bash ubuntu.sh

3. 重启电脑
4. 编译

make px4_fmu-v5_default

5.上传

make px4_fmu-v5_default upload