I have new problem. I am using pixhawk with Ardusub. How can I control the thruster with pymavlink. I need to know as soon as possible. Would you mind explaining the codes
import time
import math
# Import mavutil
from pymavlink import mavutil
# Imports for attitude
from pymavlink.quaternion import QuaternionBase
def set_target_depth(depth):
""" Sets the target depth while in depth-hold mode.
Uses https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT
'depth' is technically an altitude, so set as negative meters below the surface
-> set_target_depth(-1.5) # sets target to 1.5m below the water surface.
"""
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 z position
mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
# DON'T 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 |
# DON'T mavutil.mavlink.POSITION_TARGET_TYPEMASK_FORCE_SET |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
), lat_int=0, lon_int=0, alt=depth, # (x, y WGS84 frame pos - not used), z [m]
vx=0, vy=0, vz=0, # velocities in NED frame [m/s] (not used)
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)
)
def set_target_attitude(roll, pitch, yaw):
""" Sets the target attitude while in depth-hold mode.
'roll', 'pitch', and 'yaw' are angles in degrees.
"""
master.mav.set_attitude_target_send(
int(1e3 * (time.time() - boot_time)), # ms since boot
master.target_system, master.target_component,
# allow throttle to be controlled by depth_hold mode
mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE,
# -> attitude quaternion (w, x, y, z | zero-rotation is 1, 0, 0, 0)
QuaternionBase([math.radians(angle) for angle in (roll, pitch, yaw)]),
0, 0, 0, 0 # roll rate, pitch rate, yaw rate, thrust
)
# Create the connection
master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
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
DEPTH_HOLD = 'ALT_HOLD'
DEPTH_HOLD_MODE = master.mode_mapping()[DEPTH_HOLD]
while not master.wait_heartbeat().custom_mode == DEPTH_HOLD_MODE:
master.set_mode(DEPTH_HOLD)
# set a depth target
set_target_depth(-0.5)
# go for a spin
# (set target yaw from 0 to 500 degrees in steps of 10, one update per second)
roll_angle = pitch_angle = 0
for yaw_angle in range(0, 500, 10):
set_target_attitude(roll_angle, pitch_angle, yaw_angle)
time.sleep(1) # wait for a second
# spin the other way with 3x larger steps
for yaw_angle in range(500, 0, -30):
set_target_attitude(roll_angle, pitch_angle, yaw_angle)
time.sleep(1)
# clean up (disarm) at the end
master.arducopter_disarm()
master.motors_disarmed_wait()
I have used this code but i did not understand exactly. Would you mind explaining parametres in this code?
Thank you to your helps .