forked from monemati/PX4-ROS2-Gazebo-YOLOv8
-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathardupylot_keyboard_control.py
133 lines (102 loc) · 5.12 KB
/
ardupylot_keyboard_control.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
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
import sys
import time
from pymavlink import mavutil
from pynput import keyboard
import math
MAVLINK_CONNECTION = mavutil.mavlink_connection('udpin:localhost:14550', baud=1500000)
MAVLINK_CONNECTION.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component))
##################################################################################################################
# Manual Control Variables
roll, pitch, throttle, yaw = 0, 0, 500, 0
##################################################################################################################
def set_servo_pwm(servo_n, microseconds):
""" Sets AUX 'servo_n' output PWM pulse-width.
Uses https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_SERVO
'servo_n' is the AUX port to set (assumes port is configured as a servo).
Valid values are 1-3 in a normal BlueROV2 setup, but can go up to 8
depending on Pixhawk type and firmware.
'microseconds' is the PWM pulse-width to set the output to. Commonly
between 1100 and 1900 microseconds.
"""
# master.set_servo(servo_n+8, microseconds) or:
MAVLINK_CONNECTION.mav.command_long_send(
MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component,
mavutil.mavlink.MAV_CMD_DO_SET_SERVO,
0, # first transmission of this command
servo_n, # servo instance
microseconds, # PWM pulse-width
0,0,0,0,0 # unused parameters
)
def on_press(key):
global roll, pitch, throttle, yaw
roll, pitch, throttle, yaw = 0, 0, 500, 0
value = 600
try:
if key.char == 'r': #forward back
pitch = -value
elif key.char == 'f':
pitch = value
if key.char == 't': #left rogh
roll = value
elif key.char == 'g':
roll = -value
if key.char == 'w':
throttle = value #throtle
elif key.char == 's':
throttle = -value
if key.char == 'a':
yaw = -value # rotate
elif key.char == 'd':
yaw = value
if key.char == 'q':
print("target is left")
roll, pitch, throttle, yaw = 0, 0, 500, 0
elif key.char == 'o':
print("MODE: pos hold")
MAVLINK_CONNECTION.mav.command_long_send(MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 16, 0, 0, 0, 0, 0)
elif key.char == 'p':
print("MODE: guided no gps")
MAVLINK_CONNECTION.mav.command_long_send(MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 20, 0, 0, 0, 0, 0)
elif key.char == 'l':
print("exit")
sys.exit("Exiting the code with sys.exit()!")
except AttributeError:
print('special key {0} pressed'.format(
key))
listener = keyboard.Listener(on_press=on_press)
listener.start()
# TODO: Maybe not need! ArduPilot Should calculate home by it selfe, even with optic flow
# Manual Set HOME
#MAVLINK_CONNECTION.mav.command_long_send(MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component, mavutil.mavlink.MAV_CMD_DO_SET_HOME , 0, 0, 4, 0, 0, -35.363, 149.165, 100)
# TODO: Need set GUIDED_NOGPS MODE
# Set GUIDED mode
#MAVLINK_CONNECTION.mav.command_long_send(MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 4, 0, 0, 0, 0, 0)
# TODO: Next Time try this:
#MAVLINK_CONNECTION.mav.command_long_send(MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 20, 0, 0, 0, 0, 0)
############################################################################################3
MAVLINK_CONNECTION.mav.command_long_send(MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 0, 0, 0, 0, 0, 0)
MAVLINK_CONNECTION.mav.command_long_send(MAVLINK_CONNECTION.target_system, MAVLINK_CONNECTION.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 21196, 0, 0, 0, 0, 0)
##################################################################################################
time.sleep(0.1)
# Setup camera gimbal_2D in central position (min: 1100, max: 1900)
set_servo_pwm(5, 1500) # ROLL
set_servo_pwm(6, 1500) # TILT
time.sleep(0.1)
while True:
# # https://ardupilot.org/dev/docs/copter-commands-in-guided-mode.htmlsssss
print("thtottle: "+str(throttle)+ "; roll: "+str(roll)+"; pitch: "+str(pitch)+ "; yaw: "+str(yaw))
MAVLINK_CONNECTION.mav.manual_control_send(MAVLINK_CONNECTION.target_system, roll, pitch, throttle, yaw, 0)
time.sleep(0.1)
#while True:
# for us in range(1100, 1900, 50):
# set_servo_pwm(3, us)
# set_rc_channel_pwm(7, us)
# time.sleep(0.1)
#while True:
# for angle in range(-50, 50):
# look_at(angle*100)
# time.sleep(0.1)
# for angle in range(-50, 50):
# look_at(-angle*100)
# time.sleep(0.1)