Hello,
Goal / Issue :
I want to move forward/backward, left/right, rotate while the vehicle maintains the same depth.
I’m trying to send manual control commands to the BlueRov to control it while it’s in depth hold mode.
I’m able to control the vehicle via pymavlink by sending the manual control message while the vehicle is in Manual mode but it doesn’t work when I’m switching to Depth hold mode.
Steps :
- Disable virtual joysticks and controller in QGC
- Set Depth hold mode
- Arm
- Periodically send target depth
- Periodically send manual control
Configuration :
- BlueRov2
- BlueOS 1.1.0-beta15
- Ardusub 4.1.0
Code :
Here is the method I’m using to set the target depth. It is called in a loop with a 1Hz frequency
# Sets the target depth while in depth-hold mode
# Target must be negative
def setTargetDepth(self, target):
self._targetDepth = target
self._mavConnection.mav.set_position_target_global_int_send(
int(1e3 * (time.time() - self._bootTime)), # ms since boot
self._mavConnection.target_system, self._mavConnection.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=target, # (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)
)
Below is the method to send manual control to the vehicle, it is also called in a 1Hz loop
# Set the vehicle attitude
# Takes values in the range [-100, 100]
# MAVLink message : https://mavlink.io/en/messages/common.html#MANUAL_CONTROL
def setAttitude(self, roll, pitch, yaw, thrust):
if(roll in range(-100,100)) and (pitch in range(-100,100)) and (yaw in range(-100,100)) and (thrust in range(-100,100)):
# Normalize [-100, 100] in the range [-1000, 1000]
x_norm = pitch * 10
y_norm = roll * 10
r_norm = yaw * 10
# Normalize [-100, 100] in the range [0, 1000]
z_norm = int(((thrust + 100) / 200) * 1000)
self._mavConnection.mav.manual_control_send(
self._mavConnection.target_system,
x_norm,
y_norm,
z_norm,
r_norm,
0
)
else:
raise
The loop looks like this
# Send vehicule direction
def attitudeLoop(self):
rate = mavutil.periodic_event(1) # 1Hz
while True:
if rate.trigger():
setAttitude(0, 20, 0, 0)
self.setTargetDepth(-1)
Result :
The BlueRov follows the commands when I’m in manual mode, but it doesn’t when I switch to depth hold.
The same code works with the Ardusub SITL with the following configuration :
- Ubuntu 20.04
- ArduSub master branch
Do you have any ideas on what am I doing wrong ?
Let me know if you need more information
Best regards,
Tom
