apm-ros-optitrack初步尝试

本文记录采用ArduPilot固件，室内optitrack环境下飞行实现中遇到的一些问题

1. 在apm/mavros仿真中，总是出现mavros/state 显示 not connected。 在实际的操作中，则是显示连接的。目前的效果是 motive 通过vrpn数据送给树莓派，然后映射成vision_pose/pose给飞控，通过修改set origin可以得到融合后的local_position数据（在地面站中可以看到数据），但是在ros的话题列表中没有该话题。
2. dronekit 由于目前API较多的是针对位置环进行设置，所以无法渗透到姿态环，且对应的api太少，不适合控制
3. pymavlink 可以使用
4. 在apm-ros-sitl 中，当屏蔽GPS信号后，报错high gps hdop 选择关闭prearm gps check. 对应参数为arming_check，此时就可以解锁apm， 并尝试飞行loiter了
5. 在启动mavros之后，显示的RTT timesync to high， 可以通过修改对应的mavros加载的apm_config.yaml中的 timesync_rate 为0， 相当于不显示这个问题，其实没有解决
6. ardupilot: wrong fcu time. 由于关闭了gps, 所以没有办法获取GPS时间而报错
7. mission planner 报错： no SRTM data for this area 含义是没有该区域的地形数据。该数据是由mission planner或者mavproxy自动下载的（除了经纬度还有当地的海拔信息）。
8. 针对上述问题1和问题7，在仿真测试中，可以采用在mavproxy地图中直接设置origin来实现，在实际飞行过程中，可以采用一个脚本来完成同样的操作（实际都在发送mavlink消息过去，这样的话，在后续的matlab中，可以采用相同的思路完成这一操作）

#!/usr/bin/env python

##
#
# Send SET_GPS_GLOBAL_ORIGIN and SET_HOME_POSITION messages
#
##

import rospy
from pymavlink.dialects.v10 import ardupilotmega as MAV_APM
from mavros.mavlink import convert_to_rosmsg
from mavros_msgs.msg import Mavlink

# Global position of the origin
lat = 42.56335 * 1e7   # Terni
lon = 12.64329 * 1e7   # Terni
alt = 163 * 1e3

class fifo(object):
    """ A simple buffer """
    def __init__(self):
        self.buf = []
    def write(self, data):
        self.buf += data
        return len(data)
    def read(self):
        return self.buf.pop(0)

def send_message(msg, mav, pub):
    """
    Send a mavlink message
    """
    msg.pack(mav)
    rosmsg = convert_to_rosmsg(msg)
    pub.publish(rosmsg)

    print("sent message %s" % msg)

def set_global_origin(mav, pub):
    """
    Send a mavlink SET_GPS_GLOBAL_ORIGIN message, which allows us
    to use local position information without a GPS.
    """
    target_system = mav.srcSystem
    #target_system = 0   # 0 --> broadcast to everyone
    lattitude = lat
    longitude = lon
    altitude = alt

    msg = MAV_APM.MAVLink_set_gps_global_origin_message(
            target_system,
            lattitude, 
            longitude,
            altitude)

    send_message(msg, mav, pub)

def set_home_position(mav, pub):
    """
    Send a mavlink SET_HOME_POSITION message, which should allow
    us to use local position information without a GPS
    """
    target_system = mav.srcSystem
    #target_system = 0  # broadcast to everyone

    lattitude = lat
    longitude = lon
    altitude = alt
    
    x = 0
    y = 0
    z = 0
    q = [1, 0, 0, 0]   # w x y z

    approach_x = 0
    approach_y = 0
    approach_z = 1

    msg = MAV_APM.MAVLink_set_home_position_message(
            target_system,
            lattitude,
            longitude,
            altitude,
            x,
            y,
            z,
            q,
            approach_x,
            approach_y,
            approach_z)

    send_message(msg, mav, pub)

if __name__=="__main__":
    try:
        rospy.init_node("origin_publisher")
        mavlink_pub = rospy.Publisher("/mavlink/to", Mavlink, queue_size=20)

        # Set up mavlink instance
        f = fifo()
        mav = MAV_APM.MAVLink(f, srcSystem=1, srcComponent=1)

        # wait to initialize
        while mavlink_pub.get_num_connections() <= 0:
            pass
   
        for _ in range(2):
            rospy.sleep(1)
            set_global_origin(mav, mavlink_pub)
            set_home_position(mav, mavlink_pub)
    except rospy.ROSInterruptException:
        pass