-
Notifications
You must be signed in to change notification settings - Fork 0
/
testWriteScript.py
50 lines (43 loc) · 1.61 KB
/
testWriteScript.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
# Import mavutil
from pymavlink import mavutil
import time
# Create the connection
master = mavutil.mavlink_connection('udpin:0.0.0.0:15000')
# Wait a heartbeat before sending commands
master.wait_heartbeat()
# 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
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
# https://mavlink.io/en/messages/common.html#RC_CHANNELS_OVERRIDE
if id < 9:
# Arm
# master.arducopter_arm() or:
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)
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.
# Set some roll
set_rc_channel_pwm(2, 1600)
i = 0
while i < 20:
set_rc_channel_pwm(2, 1600)
# master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 192, 0, 4)
time.sleep(0.5)