Home        Store        Learn        Blog

Vehicle isn't executing target depth command correctly

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.

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 :slight_smile: