Home        Store        Learn        Blog

Pymavlink not working properly

So i have been working on this script a while and tested it plenty of times on BlueSub simulation, but when ever I run this python script with the vehicle it starts to get out of control and the motor is full thrust. I don’t get a chance to understand what is going on and what thrusters are working and what to as it hits anything.

Example to move ROV in straight line, with depth hold

from pymavlink import mavutil
import time
import sys

# Create the connection
master = mavutil.mavlink_connection('tcp:127.0.0.1:5760')
#master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
# Wait a heartbeat before sending commands
master.wait_heartbeat()

# Choose a mode
#Try: ['STABILIZE', 'ACRO', 'ALT_HOLD', 'AUTO', 'GUIDED', 'CIRCLE', 'SURFACE', 'POSHOLD', 'MANUAL']
mode = 'STABILIZE'

# Check if mode is available
if mode not in master.mode_mapping():
    print('Unknown mode : {}'.format(mode))
    print('Try:', list(master.mode_mapping().keys()))
    sys.exit(1)

# Get mode ID
mode_id = master.mode_mapping()[mode]
# Set new mode
# master.mav.command_long_send(
#    master.target_system, master.target_component,
#    mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0,
#    0, mode_id, 0, 0, 0, 0, 0) or:
# master.set_mode(mode_id) or:
master.mav.set_mode_send(
    master.target_system,
    mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
    mode_id)

while True:
    # Wait for ACK command
    ack_msg = master.recv_match(type='COMMAND_ACK', blocking=True)
    ack_msg = ack_msg.to_dict()

    # Check if command in the same in `set_mode`
    if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE:
        continue

    # Print the ACK result !
    print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description)
    break

#arming ROV
def is_armed():
    try:
        return bool(master.wait_heartbeat().base_mode & 0b10000000)
    except:
        return False


while not is_armed():
    master.arducopter_arm()

# Create a function to send RC values
# More information about Joystick channels
# here: https://www.ardusub.com/operators-manual/rc-input-and-output.html#rc-inputs
#channel_ID:
#5....forward & reverse---> 1900 "forward" till 1500 "stop" till 1100 "revsrese"
#6.... right or left---> 1900 "right" till 1500 "stop" till 1100 "left"
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

    rc_channel_values = [65535 for _ in range(8)]
    rc_channel_values[channel_id - 1] = pwm
    master.mav.rc_channels_override_send(
        master.target_system,
        master.target_component,
        *rc_channel_values)


while True:
    try:
        set_rc_channel_pwm(5, 1600) #x-axis control #z-axis control in real lif
        set_rc_channel_pwm(6, 1500) #y-axis control
        set_rc_channel_pwm(3, 1500) #z-axis control
        time.sleep(1.1)
        print('command sent')
    except Exception as error:
        print(error)
        sys.exit(0)

Hi @Youssefattia_98 ,

What is “BlueSub” simulation? Bluesim? or regular SITL?

Anyway, this probably means the params/eeprom is not properly configured. But I need to know what exactly you are running to be able to help you properly.

sorry, I meant that bluesim response to the script perfect but the real life ROV get’s out of control when ever run the script.

the thing is that Qground is working properly.

Hi Youssef,

Are you trying to use the ROV with a joystick connected? As per this post you likely need to disable joystick input in QGC or disconnect your joystick while you’re trying to use your script-based control, as otherwise the ROV is sent conflicting information from your script and QGC.

It may also be worth trying to use MANUAL mode (instead of STABILISE) to make sure ArduSub is only doing what you’ve asked it to, and only when you’re asking it to do so.

ok so i have tried Manuel mode in my script but it still gets out of control, also when i try to run the python script i make sure to close Qground. do i still need to disconnect the controller ?

Odd. I’d suggest trying with smaller components of the script and build up as you confirm functionality. Try

  1. just changing the mode, then
  2. add arming, then
  3. add a small thruster movement (e.g. turn on one thruster, sleep for 0.1 seconds, turn off the thruster)
  4. try your actual script

I think this should be fine without physically disconnecting the controller as long as you don’t have some other mavlink process/thread running. It may be helpful to keep QGC open and just disable the joystick from the joystick settings page though - I believe it should still give you status updates like when the ROV changes mode or gets armed (that might be wrong though).

I’ve just noticed your command loop has quite a long wait between command updates. @patrickelectric mentions (in the post I linked to earlier) that RC_OVERRIDE has a timeout controlled by RC_OVERRIDE_TIME, after which RC overrides stop applying. It’s documented here, but I’m unsure what the default timeout amount is.

It’s perhaps also worth noting that at the bottom of this post @jwalser mentions stabilise and depth-hold mode lock their angle target after 250ms of no additional control commands (he links to the source code).

hello,
I also have same issue when ever I run any python script with the vehicle it begins to urge out of control and the engine is full pushed.
please share your suggestion and help me out from this concern!
thank you in advance

Hi, @Youssefattia_98 and @Delbert_Squires59 ,

What version of Ardusub are you running? I have a vague memory of an issue with the mask value (65535) in rc_channels_override in ardupilot. I’ll see if I can find it.

I couldn’t find it. The issue was that the masked values were being accepted, making the thruster go 100% in either direction.

It should be easy to replicate by trying to set one channel to 1500. If any thruster moves, we have an issue. Would you mind trying that, and sharing a log here?

thank you Eliot will check these out and get back to you.

I am running Ardusub frimware vesion 4.0.2 and my qground control version is 4.0.5.
i would also like to mention that the bluesim is for the heavy config 8 thrusters frame while my ROV is 6 thrusters and i have choosen the 6 thrusters frame when configuring the ardusub.

also yesterday I was trying a dry test for manel mode and with rc override at channel 5 with pwm value 1550

set_rc_channel_pwm(5, 1550)

and i was expecting it to move thurster 1,2,3,4 at low rpm but instead thrusters 5 and 6 and some of the 1,2,3,4 went at max speed. i dont even get the chance to understand waht’s going on.

The BR frames for 6 thrusters vs 8 thrusters are identical except for an extra two vertical thrusters in the 8 case that control pitch, so that shouldn’t make a difference to any part of the end result other than if you were using pitch control.

This could be consistent with the ‘masked values were being accepted’ issue that @williangalvani described. Another way of testing this that might be a bit safer (but makes control a bit more annoying) is always setting all of the RC_OVERRIDE values instead of using the 65535 mask value for the ones you’re not changing. Try setting all the values to 1500, except for the one you want to be something else. Hopefully that will force the thrusters to be still other than the ones that move the ROV in your desired direction.