Home        Store        Docs        Blog

Pymavlink and rc override for autonomous guidance in ACRO mode


(Tim) #1

Thanks in advance!

Goal: to arm the vehicle and put it in a mode, say ACRO, and while in that mode have my laptop running a pymavlink script process camera images, calculate corresponding pitch, roll, yaw and thrust to send to the Pixhawk in order autonously steer the vehicle towards a target.

Approach: Using the set pwm rc override commands to transmit 1100-1900 pwm’s on rc channels 1-4.

Problem: the motors only activate for a few seconds and then stop rotating.

The following script I’m using is a compilation of scripts found here.

################################################################################

from pymavlink import mavutil

############################################################## function to send RC values
def set_rc_channel_pwm(id, pwm=1500):
    """ Set RC channel pwm value
    Args:
        id (TYPE): Channel ID
        pwm (int, optional): Channel pwm value 1100-1900
    """
    if id < 1:
        print("Channel does not exist.")
        return

    # We only have 8 channels
    #http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE
    if id < 9:
        rc_channel_values = [65535 for _ in range(8)]
        rc_channel_values[id - 1] = pwm
        master.mav.rc_channels_override_send(
            master.target_system,                # target_system
            master.target_component,             # target_component
            *rc_channel_values)                  # RC channel list, in microseconds.

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

############################################################## Arm
master.mav.command_long_send(
    master.target_system,
    master.target_component,
    mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
    0,
    1, 0, 0, 0, 0, 0, 0)

#############################################################set mode :
# Choose a mode
mode = 'ACRO'

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

# Get mode ID: return value should be 1 for acro mode
mode_id = master.mode_mapping()[mode]
master.mav.set_mode_send(
    master.target_system,
    mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
    mode_id)


############################################################# Set some ptich
set_rc_channel_pwm(1, 1600)

(Patrick José Pereira) #2

Hi Tim,

You need to send the channel value periodically.
Like:

while True:
    set_rc_channel_pwm(1, 1600)
    time.sleep(0.2)

(Tim) #3

Thanks Patrick! That works. However, is there a command to set pitch rate, roll rate, yaw rate and throttle that doesn’t require periodically updating?


(Jacob) #4

No, this is a safety mechanism. You can disable the failsafe for pilot input or gcs failsafe to override this. Be careful with that route though; it’s set up this way for a reason.


(Tim) #5

Thank you. I will be careful!

I did disable those two failsafes using QGC, however i still need to periodically set the channel at a quite fast rate in order to keep the thrusters continuously spinning. Is there something else I need to disable in order to be able to set and forget?


(Tim) #6

Is there a way to set the rc channel without having to periodically update it? I have disabled pilot input and gcs in QGround Control to no avail however.


(Jacob) #7

What version of ardusub are you using? What is the problem with updating the values at a regular interval? Can you share your code?


(Tim) #8

Hi Jacob: I have firmware version 3.5.2 installed. The interval method is computationally cumbersome for application. Is there a way to be able to bypass that requirement? As I mentioned, I disabled pilot input and gcs heartbeat.


(Jacob) #9

If you disabled the failsafes it should work. Please share your code.


(Tim) #10

Jacob, here’s the code:

from pymavlink import mavutil

mavutil.set_dialect("ardupilotmega")
# Create the connection
master = mavutil.mavlink_connection('udpin:192.168.2.1:14550')
# Wait a heartbeat before sending commands
master.wait_heartbeat()

# Set all rc chans pwm to 1500 before arming:
data = [ 1500 ] * 8
master.mav.rc_channels_override_send(
	master.target_system, master.target_component, *data)

#############################################################set mode:
# Choose a mode
mode = 'MANUAL'
# Get mode ID: return value should be 1 for acro mode
mode_id = master.mode_mapping()[mode]
# Set new mode
master.mav.set_mode_send(
master.target_system,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
mode_id)

############################################################## Arm
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
1,
1, 0, 0, 0, 0, 0, 0)
data[2] = 1600 # set some throttle 
master.mav.rc_channels_override_send(master.target_system, master.target_component, *data)

(Jacob) #11

Are you connected to QGC (with a joystick) while you are doing this?


(Tim) #12

No. Running QGC without Joystick.


(Jacob) #13

The relevant code in the firmware is here and here.

Please verify that you have disabled the failsafes.


(Jacob) #14

Also, in QGC, try looking at Widgets->Mavlink Inspector->RC_CHANNELS_RAW to see if the values match what you send, or if they change unexpectedly.


(Tim) #16

I thought QGC was allowing me to disable all failsafes however after I rebooted it shows this. The “Unknown” tabs do not have a Disabled option.


(Jacob) #17

What version of QGC are you using?


(Tim) #18

Ok after another reboot it appears all failsafes are reported disabled. However, the rc channel override still requires periodic update.


(Jacob) #19

I’ve tried your code, I had to change the link to udpin:0.0.0.0:14550. Everything is working here as I’ve indicated it should.

ArduSub 3.5.2
QGC 3.2.4 R4


(Tim) #20

Thank you Jacob! That works.


(Jacob) #21

This should not be much overhead, you only have to send the message around ~1Hz. This will ensure your ROV doesn’t run away from you during your experiments :).