Hi everyone,
Long-time user, first-time poster.
I’m trying to do precision landing using a ground-based computer. The computer measures the location of the UAV from the ground and sends landing_target mavlink messages using MAVROS.
My problem is that the PL.pX and PL.pY log topics always show 0. When I’m not sending the landing_target message, they don’t publish, and when I do send the message, they publish 0, so from that I can deduce that it is receiving the messages.
In order to get Arducopter to understand my x,y,z poition measurements, I’m reversing the maths that it does re: camera angles etc. I still need to transform them based on the heading of the UAV, but first I want to at least see the PL log topics show non-zero.
I used to have PLND_EST_TYPE set to kalman filter, and again, PL topics would not publish when I wasn’t sending landing_target messages, and when I was sending the messages, the PL topics would move around. Since the movements showed no correlation to the actual position, I set PLND_EST_TYPE to raw sensor, and now they just publish 0.
My MAVROS python code is below, and I have attached a log. I’m running Arducopter 3.6.11
I assume its a silly little thing that I’m missing, but I’m stumped. Any help would be much appreciated!
#!/usr/bin/env python
# Takes a point in space from the ZeroKey locator and publishes in Arducopter angular format to /mavros/landing_target/raw
import rospy
from geometry_msgs.msg import Point
from mavros_msgs.msg import LandingTarget
import signal
import sys
import time
import math
updated = False
zeroKeyPos = [0,0,0]
def zerokeyCallback(data):
rospy.loginfo(rospy.get_caller_id() + "\nZeroKey Location:\nx: [{}]\ny: [{}]\nz: [{}]"
.format(data.x, data.y, data.z))
global zeroKeyPos
global updated
zeroKeyPos[0] = data.x
zeroKeyPos[1] = data.y
zeroKeyPos[2] = data.z
updated = True
def listener():
rospy.init_node('listener', anonymous=True)
rospy.Subscriber('/zerokey/location', Point, zerokeyCallback)
landing_target_pub = rospy.Publisher('/mavros/landing_target/raw', LandingTarget, queue_size=1)
msg = LandingTarget()
seq = 0
global updated
while not rospy.is_shutdown():
#construct ROS topic message
if updated == True:
## for raw subscription
## must be converted to arducopter format
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.target_num = 1
msg.angle[0] = math.atan(zeroKeyPos[1])
msg.angle[1] = -math.atan(zeroKeyPos[0])
msg.distance = math.sqrt(zeroKeyPos[0]**2 + zeroKeyPos[1]**2 + zeroKeyPos[2]**2)
# Publish over ROS
landing_target_pub.publish(msg);
seq = seq + 1
updated = False
if __name__ == '__main__':
listener()
00000010.BIN (220.3 KB)