Hi. I am using a Raspberry Pi to send real time global position commands to an Ardupilot. The command method I am using is mavutil.mavlink.MAVLink_set_position_target_global_int_message. However, the requested message (master.recv_match(type='POSITION_TARGET_GLOBAL_INT', blocking=True)) from the Ardupilot suggests that only the altitude target is set successfully. I have also tried to send commands to a fixed-wing drone in SITL, and the result is the same.
The code is as follows:
import time
# Import mavutil
from pymavlink import mavutil
import numpy as np
master = mavutil.mavlink_connection('tcp:localhost:5762', baud=57600)
# Make sure the connection is valid
master.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" %
(master.target_system, master.target_component))
# Run the acquisition
run = True
# Time at boot
t0 = time.time()
while run:
#try:
attitude_dat = master.recv_match(type='ATTITUDE', blocking=True)
roll_pitch_yaw_raw = np.array([attitude_dat.roll, attitude_dat.pitch, attitude_dat.yaw])
roll_pitch_yaw = np.rad2deg(roll_pitch_yaw_raw)
position_dat = master.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
lat_lon = np.array([position_dat.lat, position_dat.lon]) / 1e7 # Conversion factor
# Get relative altitude and vertical speed
alt_vz = np.array([position_dat.relative_alt, position_dat.vz]) / 1e3
# Ensure time_boot_ms is an integer (milliseconds since boot)
time_boot_ms = int((time.time() - t0) * 1000)
master.mav.send(mavutil.mavlink.MAVLink_set_position_target_global_int_message(
time_boot_ms, # time_boot_ms should be an integer
master.target_system,
master.target_component,
5, 3576,
353621474, # Ensure latitude is an integer
1491651746, # Ensure longitude is an integer
45, # Flight height in meters
0, 0, 0, 0, 0, 0, 0, 0))
pos_setpoint = master.recv_match(type='POSITION_TARGET_GLOBAL_INT', blocking=True)
print('====================================')
print(pos_setpoint.to_dict())
print('====================================')
print(f'Roll angle: {roll_pitch_yaw[0]}, Pitch angle: {roll_pitch_yaw[1]}, Yaw angle: {roll_pitch_yaw[2]}')
print(f'Latitude: {lat_lon[0]}, Longitude: {lat_lon[1]}')
print(f'Height: {alt_vz[0]}, Vert speed: {alt_vz[1]}')
The output looks like this:
{'mavpackettype': 'POSITION_TARGET_GLOBAL_INT', 'time_boot_ms': 280344, 'coordinate_frame': 0, 'type_mask': 65016, 'lat_int': (home position), 'lon_int': (home position), 'alt': 45.0, 'vx': 0.0, 'vy': 0.0, 'vz': 0.0, 'afx': 0.0, 'afy': 0.0, 'afz': 0.0, 'yaw': 0.0, 'yaw_rate': 0.0}
====================================
Roll angle: 50.12220941273635, Pitch angle: -1.1132439950839157, Yaw angle: -107.35024027944132
Latitude: (my position), Longitude: (my position)
Height: 42.7, Vert speed: 0.0
The target coordinates are not set properly and are for the home location.
I have even tried the following lines from MAVProxy, but nothing changes but the altitude target:
master.mav.set_position_target_global_int_send(
time_boot_ms, # time_boot_ms should be an integer
master.target_system,
master.target_component,
5, 3576,
353621474, # Ensure latitude is an integer
1491651746, # Ensure longitude is an integer
45, # Flight height in meters
0, 0, 0, 0, 0, 0, 0, 0)
Any help will be greatly appreciated.