Hi all, I’ve got a custom frame vectored 6dof style auv. I am using the same setup from the blue robotics electronics enclosure. I am currently trying to hardcode some movement in it using pymavling, I want it to go a little bit underwater (-0.6m) and move forwards for a set amount of time and then come back up. The only external sensor I currently have is a Bar30 pressure sensor, I’m hoping to rely on the internal compass for attitude control and rc_channel_override for position control.
This is the code I currently have
import time
import signal
import sys
import math
from pymavlink.quaternion import QuaternionBase
from pymavlink import mavutil
from pprint import pprint
# GLOBAL
boot_time: float = 0.0
def wait_connection() -> dict:
"""
Waits for a mavlink connection to occur with the rov.
"""
print("waiting connection")
msg = None
while not msg:
master.mav.ping_send(
int(time.time() * 1e6), # Unix time in microseconds
0, # ping count
0, # request ping of all systems
0, # request ping of all components
)
msg = master.recv_match()
time.sleep(0.5)
print("connected")
return msg.to_dict()
def arm_vehicle() -> None:
"""
Arms the vehicle for actuation to occur.
"""
# master.mav.command_long_send(
# master.target_system,
# master.target_component,
# mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
# 0, # confirmation
# 1, 0, 0, 0, 0, 0, 0 # parameters (1 for arm)
# )
print("arming")
master.arducopter_arm()
master.arducopter_arm()
master.arducopter_arm()
master.arducopter_arm()
master.motors_armed_wait()
print("armed")
def disarm_vehicle() -> None:
"""
Disarms the vehicle, disabling actuation.
"""
if master:
print("disarming")
master.arducopter_disarm()
master.arducopter_disarm()
master.arducopter_disarm()
master.motors_disarmed_wait()
print("disarmed")
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
)
def set_rc_channel_pwm(channel_id, pwm=1500):
""" Set RC channel pwm value
Args:
channel_id (TYPE): Channel ID
pwm (int, optional): Channel pwm value 1100-1900
"""
if channel_id < 1 or channel_id > 18:
print("Channel does not exist.")
return
# mavlink 2 supports up to 18 channels:
# https://mavlink.io/en/messages/common.html#RC_CHANNELS_OVERRIDE
rc_channel_values = [65535 for _ in range(18)]
rc_channel_values[channel_id - 1] = pwm
master.mav.rc_channels_override_send(
master.target_system, # target_system
master.target_component, # target_component
*rc_channel_values) # RC channel list, in microseconds.
# def dive() -> No
def change_control_mode(mode: str) -> None:
"""
Changes the control mode of the rov,
valid modes are:
MANUAL
STABILIZE
ALT_HOLD (depth-hold mode)
"""
mode_id = master.mode_mapping()[mode]
while not master.wait_heartbeat().custom_mode == mode_id:
master.mav.heartbeat_send(
6, # gcs unit
8, # autopilot mode
192, # base_mode
0, # custom_mode
4, # system_status
3 # mavlink_version
)
print(f"Attempting to set mode {mode}")
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_DO_SET_MODE,
0,
1, mode_id, 0, 0, 0, 0, 0)
print(f"set mode {str}")
def run_control(duration: float, depth: float, forward_pwm: int) -> None:
"""
Runs autonomous control loop for the rov.
"""
# change_control_mode('ALT_HOLD')
# curr_time: float = 0.0
set_target_depth(depth)
# set_rc_channel_pwm(5, forward_pwm)
end_time = time.time() + duration
count = 0
while time.time() < end_time:
print(f"moving forwards at {time.time()}s")
set_target_attitude(0,0,90)
# curr_time += 0.1
count += 0.1
time.sleep(1)
set_target_depth(depth)
if (count == 1):
count = 0
set_target_depth(-0.2)
set_rc_channel_pwm(5, 1500)
def exit_gracefully(a, b):
disarm_vehicle()
sys.exit(-1)
signal.signal(signal.SIGINT, exit_gracefully)
signal.signal(signal.SIGTERM, exit_gracefully)
# Connect to mavlink
master = mavutil.mavlink_connection('udpout:0.0.0.0:14550')
boot_time = time.time()
data = wait_connection()
master.wait_heartbeat()
print("waited")
pprint(data)
arm_vehicle()
change_control_mode("ALT_HOLD")
# time.sleep(10)
set_target_depth(-0.6)
time.sleep(2)
run_control(duration=5.0, depth=-0.6, forward_pwm=1700)
disarm_vehicle()
It’s an amalgumation of the pymavlink examples in the docs. I have very limited time in the water before I have to get this working so I just want to sanity check my code and assumptions before I get to test.
any help would be appreciated!! Thanks!