pymavlink speed ardusub

speed_control.py

"""
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()