Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
pymavlink speed ardusub
"""
Example of how to set target speed in guided mode with pymavlink
"""
import time
import math
# Import mavutil
from pymavlink import mavutil
# Imports for attitude
from pymavlink.quaternion import QuaternionBase
def set_target_speed(vx, vy, vz):
""" Sets the target speed in Guided mode.
Uses https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT
"""
master.mav.set_position_target_global_int_send(
int(1e3 * (time.time() - boot_time)), # ms since boot
master.target_system, master.target_component,
coordinate_frame=mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
type_mask=( # ignore everything except x, y, & z positions
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_VX_IGNORE |
# mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
# mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE
), lat_int=0, lon_int=0, alt=0, # (x, y WGS84 frame pos - not used), z [m]
vx=vx,
vy=vy,
vz=vz,
afx=0, afy=0, afz=0, yaw=0, yaw_rate=0
# accelerations in NED frame [N], yaw, yaw_rate
# (all not supported yet, ignored in GCS Mavlink)
)
# Create the connection
master = mavutil.mavlink_connection('udpin:0.0.0.0:14555')
boot_time = time.time()
# Wait a heartbeat before sending commands
master.wait_heartbeat()
# arm ArduSub autopilot and wait until confirmed
master.arducopter_arm()
master.motors_armed_wait()
# set the desired operating mode
GUIDED = 'GUIDED'
GUIDED_MODE = master.mode_mapping()[GUIDED]
while not master.wait_heartbeat().custom_mode == GUIDED_MODE:
master.set_mode(GUIDED)
while True:
set_target_speed(0.0, 10.0, 0.0)
# clean up (disarm) at the end
master.arducopter_disarm()
master.motors_disarmed_wait()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment