本文记录采用ArduPilot固件,室内optitrack环境下飞行实现中遇到的一些问题
- 在apm/mavros仿真中,总是出现mavros/state 显示 not connected。 在实际的操作中,则是显示连接的。目前的效果是 motive 通过vrpn数据送给树莓派,然后映射成vision_pose/pose给飞控,通过修改set origin可以得到融合后的local_position数据(在地面站中可以看到数据),但是在ros的话题列表中没有该话题。
- dronekit 由于目前API较多的是针对位置环进行设置,所以无法渗透到姿态环,且对应的api太少,不适合控制
- pymavlink 可以使用
- 在apm-ros-sitl 中,当屏蔽GPS信号后,报错high gps hdop 选择关闭prearm gps check. 对应参数为arming_check,此时就可以解锁apm, 并尝试飞行loiter了
- 在启动mavros之后,显示的RTT timesync to high, 可以通过修改对应的mavros加载的apm_config.yaml中的 timesync_rate 为0, 相当于不显示这个问题,其实没有解决
- ardupilot: wrong fcu time. 由于关闭了gps, 所以没有办法获取GPS时间而报错
- mission planner 报错: no SRTM data for this area 含义是没有该区域的地形数据。该数据是由mission planner或者mavproxy自动下载的(除了经纬度还有当地的海拔信息)。
- 针对上述问题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
评论(0)
您还未登录,请登录后发表或查看评论