Manual control not working in depth hold mode

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

1 Like

Hi @TomRvr, welcome to the forum :slight_smile:

Apologies for the delay on getting to this.

To clarify your situation,

  1. What do you mean by “it doesn’t work when I’m switching to Depth hold mode”?
    • Are your control commands ignored?
    • Does it move unexpectedly?
  2. Does controlling the vehicle in depth hold mode work normally (e.g. via QGroundControl, with a joystick)?
  3. Are you also sending regular HEARTBEAT messages?
    • You’re already sending regular motion control messages, so the pilot input side of things should be ok

Are you saying the issue is only present in the ArduSub 4.1.0 release, and has been fixed in the master branch? If so, can you try checking out the ArduSub-4.1.1 tag (or just the Sub-4.1 branch) and seeing whether it works as expected there?

Hi,
Thanks for your reply

  1. The control commands are ignored
    The vehicle holds its depth but doesn’t move

  2. Controlling the vehicle with a joystick in depth hold mode works normally

  3. I’m not sending regular HEARTBEAT messages
    Thanks for the suggestion, I will add a loop to send them at 1Hz
    Is the failsafe not present in manual mode (my control commands are working in this mode) ?
    Is it necessary to disable QGC virtual joysticks or real joystick to allow the control commands ?

  4. I’m using ArduSub 4.1.0 on the vehicle and ArduSub master branch in simulation
    I will switch to the 4.1.1 version on both, and I will let you know how it goes
    I see it’s a beta version, any risk of using it with the hardware ?

Here is a simple code I made to make testing easier, it only sends heartbeat, depth and manual control :

from pymavlink import mavutil
import time, threading

hbRate = mavutil.periodic_event(1) # 1Hz 
cmdRate = mavutil.periodic_event(1)

# Sets the target depth while in depth-hold mode
# Target must be negative
def setTargetDepth(target):
    mavConnection.mav.set_position_target_global_int_send(
        int(1e3 * (time.time() - bootTime)), # ms since boot
        mavConnection.target_system, 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)
    )

# Set the vehicle attitude
# Takes values in the range [-100, 100]    
# MAVLink message : https://mavlink.io/en/messages/common.html#MANUAL_CONTROL 
def setAttitude(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)
        mavConnection.mav.manual_control_send(
            mavConnection.target_system,
            x_norm,
            y_norm,
            z_norm,
            r_norm,
            0
        )

def heartbeatLoop():
    while True:
        if hbRate.trigger():
            mavConnection.mav.heartbeat_send(
                mavutil.mavlink.MAV_TYPE_GCS,
                mavutil.mavlink.MAV_AUTOPILOT_INVALID,
                0,
                0,
                0
            )

def cmdLoop():
    while True:
        if cmdRate.trigger():
            setTargetDepth(-1)
            setAttitude(0, 20, 0, 0)

            
# Connect to the vehicle
mavConnection = mavutil.mavlink_connection('udpin:localhost:14552') # SITL
# mavConnection = mavutil.mavlink_connection('udpout:0.0.0.0:9001') # BlueRov
msg = None
elapsed = 0
timeout = 10
while not msg:
    mavConnection.mav.ping_send(
        int(time.time() * 1e6),
        0,
        0,
        0
    )
    msg = mavConnection.recv_match()
    time.sleep(0.5)
    elapsed += 0.5
    if elapsed >= timeout:
        print("Failed to connect")
        exit(1)

bootTime = time.time()

mavConnection.arducopter_disarm()
mavConnection.motors_disarmed_wait()

# Start sending heartbeat
hbThread = threading.Thread(target= heartbeatLoop, daemon= True)
hbThread.start()

# Set depth hold mode
map = None
while map==None:
    map = mavConnection.mode_mapping()

DEPTH_HOLD_MODE = map['ALT_HOLD']
while not mavConnection.wait_heartbeat().custom_mode == DEPTH_HOLD_MODE:
    mavConnection.set_mode('ALT_HOLD')

# Send depth and position commands
mavConnection.arducopter_arm()
mavConnection.motors_armed_wait()

cmdLoop()

I tested it in SITL and control commands are working on master and Sub-4.1 with or without heartbeat (no virtual joystick and no physical joystick)

I will let you know the results when I’ll be able to test it on the vehicle

1 Like

That’s really weird, because the joystick actions are conveyed to the vehicle using MANUAL_CONTROL messages. I’m not sure why that wouldn’t be working, but it’s also interesting that it’s apparently fixed in the beta and development branches.

The heartbeat failsafe doesn’t care which control mode the vehicle is in. If you have a MAVLink connection to QGC at the same time as your pymavlink program then the vehicle will be receiving heartbeats from QGC, which would prevent the failsafe from occurring. QGC’s heartbeats can be turned off from the Application Settings / MAVLink page.

There may also be control conflicts if QGC is open and has a joystick connected. If that’s the case I’d recommend either disconnecting the joystick, changing QGC’s MAVLink System ID off 255 (so the vehicle ignores control commands from it), or just having QGC closed when you’re using programmatic control.

Beta means “we think this should work well, but want additional testing before deciding it’s stable enough to be recommended for every vehicle”.

There’s a chance of bugs in any software release, but that chance is lower for stable releases (because they’ve undergone more testing), and if a bug does show up it’s less likely it will be significant to common operating behaviours and requirements.

I’ve also used a thread-based heartbeat in this example, but it’s worth noting that

Thanks for the details

I did some testing on the vehicle, here are some results :

ArduSub 4.1.1 beta (66c674c1)

  • No pymavlink heartbeat, QGC running with joystick enabled

Depth hold : ok (vibrations)
Pymavlink manual control : Not ok (Doesn’t move)

  • No pymavlink heartbeat, QGC running without joystick

Depth hold : ok (vibrations)
Pymavlink manual control : Move ok

  • With pymavlink heartbeat, QGC running no joystick

Depth hold : ok (vibrations)
Pymavlink manual control : Move ok

  • With heartbeat, QGC running with joystick

Depth hold : ok (vibrations)
Pymavlink manual control : Doesn’t move

ArduSub 4.1.0 stable (f2af3c7e)

  • No pymavlink heartbeat, QGC running no joystick

Depth hold : ok
Pymavlink manual control : Move ok

  • With pymavlink heartbeat, QGC not running

Depth hold : ok
Pymavlink manual control : Move ok

Observations

  • In version 4.1.1, the vehicle oscillates around the target depth when in depth hold mode
  • QGC joystick must be disabled
  • Heartbeat must be sent to allow manual control without QGC running
  • The issue I faced before was probably related to QGC interference, not sending heartbeat, not sending manual control fast enough, and too small manual control values

Questions

  • What is the recommended frequency for sending MANUAL_CONTROL messages ?
  • How are the manual control values (x, y, z…) interpreted in depth hold mode ? I’m not sure to understand if it controls the speed, the power… I saw this but it’s still unclear to me

Your presented results all seem to make sense, from what I can tell :slight_smile:

Hmm, is your vehicle completely stock, or have you modified it / added something that would change the weight distribution? It may help to try recalibrating the accelerometer, and possibly adjusting the PSC_xxxZ_P/D parameters to tune the vertical PID controllers. Reducing P (proportional) terms slows the response but should reduce vibrations by avoiding overshooting. Increasing D (derivative) terms should reduce oscillations by reducing all high frequency changes.

It just needs to have a shorter period than the shorter of your FS_PILOT_TIMEOUT and RC_OVERRIDE_TIME parameter values.

Higher frequency can improve the responsiveness if you need to do precise movements, but too frequent isn’t possible for the vehicle to respond to and just wastes bandwidth. I expect there’ll be diminishing returns beyond ~10Hz, especially without requesting higher sensor update rates in the telemetry stream.

If the roll-pitch toggle has not been activated then the x/y values are interpreted the same way in depth hold as they are in manual mode, as forward/lateral motor contributions. If the toggle has been activated manual mode treats x/y as roll/pitch rotation rates, whereas depth hold treats them as roll/pitch angles.

In manual mode z/r correspond to the vertical/heading motor contributions. In depth hold z/r can be considered as a strength of desire to go up or down/left or right, and sets a new depth/heading target as the current value when the corresponding joystick axis is released.

mavConnection = mavutil.mavlink_connection(‘udpout:0.0.0.0:9001’) # BlueRov

Have you configured anything in “MAVLink Endpoints” for this?

Yes, I configured BlueOS to add a MAVLink UDP server
It looks like this :