Vehicle isn't executing target depth command correctly

[VioletCheese (ATP)]

Hello again.

I am trying to code a specific target depth. I am using pymavlink to send a target depth command as per the code below. It seems to try to go down to the specified depth but the motors aren’t running continuously. Our vehicle has a density lower than water so our vehicle goes down and up without actually reaching the target depth.

I will post a link to a video of the vehicle to maybe help with diagnosing as well as the code.
I also ran a modified version of this code where the depth target command is sent inside the while loop.-Same thing happened.
Also after the execution of the code, when it comes time to disarm the vehicle, it doesn’t respond. The code waits in the master.motors_disarmed_wait() section until I manually interrupt the code.
has anyone else experienced a similar problem?
I am also sending a yaw value of 60%clockwise and it is not performing a yaw rotation at all.

The Code:

from pymavlink import mavutil
import time


master = mavutil.mavlink_connection("udpin:192.168.2.1:14550")
master.wait_heartbeat()

boot_time = time.time()
print("Successful Connection")


def set_target_depth(depth):

    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=0xdfb, 
        lat_int=0, lon_int=0, alt=depth,
        vx=0, vy=0, vz=0, 
        afx=0, afy=0, afz=0, yaw=0, yaw_rate=0
    
    )


def send_pwm(x =0, y=0 , z = 500, yaw=0 , buttons=0):
    """Send manual pwm to the axis of a joystick. 
    Relative to the vehicle
    x for right-left motion
    y for forward-backwards motion
    z for up-down motion, z -> {0-1000}
    r for the yaw axis
        clockwise is -1000
        counterclockwise is 1000
    buttons is an integer with 
    """
    print(f"running for  seconds, with pwm x:{x},y={y}, z={z} \n yaw:{yaw}, buttons:{buttons}")

    master.mav.manual_control_send(master.target_system, x,y,z,yaw,buttons)


##Arm Vehicle
master.arducopter_arm()
master.motors_armed_wait()

#Flight Mode Setup
FLIGHT_MODE = 'ALT_HOLD'
FLIGHT_MODE_ID = master.mode_mapping()[FLIGHT_MODE]

while not master.wait_heartbeat().custom_mode == FLIGHT_MODE_ID:
    master.set_mode(FLIGHT_MODE)


##Run motors for 20 seconds
tstart = time.time()
set_target_depth(-1.5)
while time.time()<tstart+20:
    
    send_pwm(yaw = 600)

master.arducopter_disarm()
master.motors_disarmed_wait()

The Video Link:
IMG_3424.MOV - Google Drive

Our Power Setup:
We are using a 12V 60A adapter and powering the vehicle directly from the wall. a further voltage regulator brings the voltage to 5V for use in the raspberry and pixhawk. We are using a XT90 pixhawk power module to power the 6 T200 thrusters.

Control setup.
we are using a raspberry as a companion, the Pixhawk as the autopilot and ardusub. I am running pymavlink on my ubuntu computer with python 3.8.10

I can provide further information if necessary.

I had asked this question as a reply to one of the posts I had made earlier so I am putting it here so it can be found easier.

[EliotBR (Eliot)]

Have you tried running it without the yaw command? Your current manual control message is always specifying values for horizontal and vertical movement together with the yaw rotation, which means depth-hold mode will then wait for the vertical commands to stop, after which it accepts the current depth as the target to maintain. When you have the ‘set depth target’ in the while loop with the yaw command then they’ll be fighting with each other.

The mavlink docs for MANUAL_CONTROL specify that INT16_MAX (2^{15} - 1 = 32767) indicates a given axis is invalid, so you may be able to specify that for the ones you don’t want to give explicit values for. If that doesn’t work then it should at least be possible to use RC_CHANNELS_OVERRIDE instead, since that explicitly ignores channels that are set to UINT16_MAX (2^{16}-1 = 65535), which is what we use in our Send RC (Joystick) Pymavlink example.

If neither of those approaches work then please confirm your companion, ArduSub, and Pymavlink versions so we can try to troubleshoot what’s going on.

By the way, this post is quite confusingly formatted. I’d suggest you have a look through the Formatting a Post/Comment section from my recent “How to Use the Blue Robotics Forums” post - you’re likely interested in the “Headings” subsection

[VioletCheese (ATP)]

Hello,
I am trying to use the manual control command, but let pixhawk stabilize the depth using the set_target_depth command from the pymavlink documentation. But, sending a z=500 tells the depth thrusters to stop. I searched the documentation and found that a value of 32767 (INT_MAX) disables an axis.
If I send a command like (x=0,y=0,yaw=400, z=32767, buttons=0) after sending a set_target_depth(-0.5), will the vehicle JUST turn clockwise with 40% power, or go down to -0.5meters as well? (Assuming a functioning pressure sensor and depth hold mode active)
I wasn’t sure of sending 32767 because it is a large number and there was a legacy workaround thing going on with the z axis.
If this isn’t the correct approach, I would be happy if you could let me know what could be used so that I can “delegate” the z axis to pixhawk while retaining control over the other axes.
Last year, I couldn’t find a solution, so I just wrote a pid to control the z axis by reading the pressure sensor myself, but it was significantly worse than the default pid that comes with ardusub.
Thanks for your time

[EliotBR (Eliot)]

Hi @VioletCheese,

I’ve moved this here since it’s on the same topic.

Having looked into this a bit more, it seems like ArduSub doesn’t do anything special for INT16_MAX values in its manual control handling (which was apparently raised in an issue a while ago), so to get “ignore” behaviour requires using RC_CHANNELS_OVERRIDE instead.

That said, ArduSub’s depth hold code apparently uses the last sent throttle value as part of its calculation of the pilot’s desired climb rate, so avoiding sending new values is potentially less useful than sending zero-throttle values (if the desired rate is calculated as ~0 then the depth is maintained at that depth).

It’s worth noting that the climb rate calculation code treats the pilot inputs as in the body frame of the vehicle, so depending on the vehicle’s rotation forward or lateral commands could make sufficient vertical contributions to trigger a change of depth target. I expect the only ways to get around that are to send regular depth target messages at your actual desired level, and/or have a sufficiently balanced vehicle / slow enough movements that it doesn’t have strong rotations that change the target. Best performance is presumably with a well-balanced vehicle and regular target commands to ensure it is maintained.

@VioletCheese

If I send a command like (x=0,y=0,yaw=400, z=32767, buttons=0) after sending a set_target_depth(-0.5), will the vehicle JUST turn clockwise with 40% power, or go down to -0.5meters as well? (Assuming a functioning pressure sensor and depth hold mode active)

Given this example, it’s perhaps also worth mentioning that the SET_ATTITUDE_TARGET message can set attitude rates instead of set angle values if you want it to (that’s also commented in our depth/attitude target pymavlink example).

---

My links to the ArduSub code base here are to the ArduSub-stable tag, which we’ve very recently updated from 4.0.3 to 4.1.0. I’ll be making a post about the main differences in the next couple of days, but there are a few depth hold improvements included (you can see the release notes).

[ROVenjoyer (M)]

Hi guys,
my problem might be related to this topic.

I encountered a problem, when using the depth hold mode in combination with ‘set_position_target_global_int_send()’

My setup:
I am using the SITL provided here: SITL · GitBook
I am running it natively on Windows using Cygwin.

I am doing the following steps:
Setting up SITL

1. Open QGroundControl
2. Open the Cygwin64 Terminal
3. Navigate to ‘~/ardupilot/tools/autotest’
4. Execute ‘python3.7 sim_vehicle.py -v ArduSub -L RATBeach --out=udp:0.0.0.0:14550 --map --console’
5. MAVProxy opens
6. Execute ‘output add 127.0.0.1:14551’
   Run my Code
7. Run Set Target Depth/Attitude Example: Pymavlink · GitBook
   This is my script.
   I basically copy and pasted the example. I only changed the target depth from -0.5m to -10m and the udpin from ‘udpin:0.0.0.0:14550’ to the in step 5) added ‘udpin:127.0.0.1:14551’

My problem
I am watching the state of the sub on QGroundControl, where it displays an unexpected behaviour. It either does not hold the specified depth or it does not even dive to the specified depth in the first place.

My approach to solve this problem
I tried a bunch of stuff, like looping ‘set_position_target_global_int_send()’ until it actually reaches the desired depth. Although Eliot stated in this post:

Regarding: pwm signal delays, setting target depth, type_masks and NED system, setting target attitude, heartbeat message

This depends a lot on the code you’re using, the particular operations you need to do, and the hardware you’re running it on. It’s very difficult to comment in a meaningful way without access to that information, although I understand that since this is for a competition you may not be willing/able to share those details. As with every optimisation task, I’d suggest a good starting point is to profile your code, and see where the most time is being spent. As a general principle for Python appl…

it should be set only once.
But my sub dived to the specified depth, only if i looped it.

But once i start rotating, the sub starts to raise to the surface again, eventhough it is in depth hold mode and i did not change the target depth. All i did, was to use the ‘set_attitude_target_send()’ function, with the ‘ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE’ as used in the previously mentioned example.

I do not know how to debug this either. For example i tried to request the message for ‘POSITION_TARGET_GLOBAL_INT’ to check if the target depth is actually set, but i received no answer. But i can receive other messages like ‘GLOBAL_POSITION_INT’.
This is my current project where i tried to use the workaround by looping ‘set_position_target_global_int_send()’ in Testsequence_SurfaceComputerToAutopilot_w_set_target_position(). It is quite messy though and i do not expect anyone to solve this for me.

What i am ultimately trying to achieve
I am trying to autonomously dive to a specified depth, rotate and make some pictures. That should be an easy task, but i am having a hard time, even with the provided examples.
I can not test it on the actual submarine for now, thats why i tried to make it work in the SITL first.

I found a bunch of forum entries, that are related to the topic, unfortunately most of them are a dead end to me.