Hi, I am trying to send velocity commands through this script on Ardupilot, SITL. However, I am unable to see any kind of movement when the commands/messages are being sent. Can anyone help out by identifying what could possibly be missing or incorrect. For comparison purposes, I am able to control my copter’s attitude through it’s command/message.
import time
import math
# Import mavutil
from pymavlink import mavutil
import numpy as np
def set_target_velocity(velocity_x, velocity_z):
msg = master.mav.set_position_target_global_int_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame
# 0b0000111111000111, # type_mask (only speeds enabled)
mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_FORCE_SET |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE,
0, # lat_int - X Position in WGS84 frame in 1e7 * meters
0, # lon_int - Y Position in WGS84 frame in 1e7 * meters
0, # alt - Altitude in meters in AMSL altitude(not WGS84 if absolute or relative)
# altitude above terrain if GLOBAL_TERRAIN_ALT_INT
velocity_x, # X velocity in NED frame in m/s
0, # Y velocity in NED frame in m/s
velocity_z, # Z velocity in NED frame in m/s
0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
#send message at 10 hz cycle at a step of 0.1
for x in np.arange(0.0, 100, 0.1):
# print(x)
master.mav.send(msg)
# print(f"{velocity_x,velocity_z}")
time.sleep(0.1)
master = mavutil.mavlink_connection('127.0.0.1:14550')
# Wait a heartbeat before sending commands
master.wait_heartbeat()
master.set_mode('GUIDED')
print("Mode changed --")
time.sleep(1)
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
1, 0, 0, 0, 0, 0 ,0
)
print("waiting for vehicle to arm")
master.motors_armed_wait()
print("Armed")
time.sleep(1)
# time.sleep(1)
print("taking off")
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0,
0, 0, 0, 0, 0, 0, 50
)
time.sleep(30)
# commanding velocity
print("commanding")
set_target_velocity(velocity_x= 10,velocity_z=2)
# landing
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_NAV_LAND,
0,
0, 0, 0, 0, 0, 0 ,0
)
print("landing")
time.sleep(30)
# disarming vehicle
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
0 , 0, 0, 0, 0, 0 ,0
)
master.motors_disarmed_wait()
Can someone also try running this script on SITL and get back if they had the same issue.