Skip to content

Commit 61a068c

Browse files
alireza787balireza787b
committed
velocity GUI
1 parent 9294141 commit 61a068c

File tree

3 files changed

+142
-84
lines changed

3 files changed

+142
-84
lines changed

‎examples/offboard_from_stream/body_velocity_sender_example.py

Lines changed: 133 additions & 78 deletions
Original file line numberDiff line numberDiff line change
@@ -1,53 +1,82 @@
11
"""
2-
Drone Velocity and Yaw Control Sender
3-
=====================================
4-
5-
This script enables real-time control of a drone's velocity and yaw using keyboard inputs.
6-
It sends velocity setpoints in the body frame and commands for yaw adjustments, allowing
7-
for dynamic control through familiar keyboard keys.
8-
9-
Key Bindings:
10-
- 'W': Move forward
11-
- 'S': Move backward
12-
- 'A': Move left
13-
- 'D': Move right
14-
- Up Arrow: Increase altitude
15-
- Down Arrow: Decrease altitude
16-
- Left Arrow: Yaw left
17-
- Right Arrow: Yaw right
18-
- 'Q': Quit the application
19-
20-
The drone must be configured to receive and process these commands, typically in an offboard control mode.
2+
MAVSDK Offboard Control - Local Velocity Sender
3+
=================================================
4+
5+
This script is designed to control a drone's velocity and yaw through keyboard inputs, interfacing via MAVSDK over UDP. It offers an interactive GUI built with Pygame for real-time control and feedback.
6+
7+
Overview:
8+
---------
9+
- Sends control packets to command drone movements in local body coordinates with optional yaw adjustments.
10+
- Utilizes keyboard inputs for dynamic control (W, A, S, D, Arrow keys).
11+
- Real-time feedback on current command and connection status via a Pygame-based GUI.
12+
13+
Setup Requirements:
14+
-------------------
15+
- A MAVSDK-compatible drone or a SITL setup running and accessible on the network.
16+
- The receiver node (`receiver.py`) must be operational to handle and execute the commands sent from this script.
17+
- Ensure that the receiver and this sender script are configured to communicate over the specified IP and port.
18+
19+
Key Functionalities:
20+
--------------------
21+
- **Velocity Control**: Use W, A, S, D for movement along the body frame's axes.
22+
- **Altitude Adjustment**: Use the Up and Down arrow keys for vertical movement adjustments.
23+
- **Yaw Control**: Use Left and Right arrow keys to adjust yaw.
24+
- **Enable/Disable Commands**: Toggle command sending with 'E' to enable and 'C' to cancel.
25+
- **Emergency Stop**: 'H' to hold (stop all movements) and 'Q' to quit the application safely.
26+
27+
28+
Instructions for Use:
29+
---------------------
30+
1. Ensure your MAVSDK setup (either SITL or a real drone) is ready and the `receiver.py` script is running.
31+
2. Adjust parameters such as IP, port, and control rates directly in the code if necessary.
32+
3. Run this script in a Python environment where Pygame is installed.
33+
4. Use the keyboard controls as outlined to command the drone.
34+
35+
Safety Notice:
36+
--------------
37+
- When using with a real drone, operate in a safe, open environment to avoid accidents.
38+
- Always be ready to take manual control or stop the drone if necessary.
2139
2240
Author:
2341
- Alireza Ghaderi
2442
- GitHub: alireza787b
2543
- Date: April 2024
26-
w
27-
Usage:
28-
Run the script in a compatible Python environment where the drone control system can
29-
receive UDP packets. Use the keys to control the drone's movement and orientation.
44+
45+
Dependencies:
46+
- Pygame for GUI operations.
47+
- Python's `socket` library for UDP communication.
48+
- `control_packet.py` for formatting control commands.
3049
"""
3150

3251
import socket
52+
import pygame
3353
import sys
34-
import select
35-
from control_packet import ControlPacket, SetpointMode
36-
37-
import socket
38-
import keyboard # Using the keyboard library for non-blocking key reads
3954
from control_packet import ControlPacket, SetpointMode
4055

4156
# Constants for communication and control
4257
UDP_IP = "127.0.0.1"
4358
UDP_PORT = 5005
59+
SEND_RATE = 0.1 # Packet send rate in seconds (10 Hz)
4460
DEFAULT_SPEED = 1.0 # meters per second
45-
DEFAULT_YAW_RATE = 5.0 # degrees per second, incremental
61+
YAW_RATE_STEP = 10.0 # degrees per step
62+
63+
# Initialize Pygame and set up the display
64+
pygame.init()
65+
screen = pygame.display.set_mode((800, 600))
66+
pygame.display.set_caption('MAVSDK Offboard Control - Local Velocity Sender')
67+
68+
# Colors, fonts, and initial settings
69+
BACKGROUND_COLOR = (30, 30, 30)
70+
TEXT_COLOR = (255, 255, 255)
71+
FONT = pygame.font.Font(None, 36)
72+
SMALL_FONT = pygame.font.Font(None, 24)
73+
GREEN = (0, 255, 0)
74+
RED = (255, 0, 0)
4675

4776
# Setup UDP socket for sending commands
4877
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
4978

50-
def send_velocity_body(velocity_x, velocity_y, velocity_z, yaw_rate=0):
79+
def send_velocity_body(velocity_x, velocity_y, velocity_z, yaw_rate):
5180
"""Send a velocity command with optional yaw rate to the drone."""
5281
packet = ControlPacket(
5382
mode=SetpointMode.VELOCITY_BODY,
@@ -56,60 +85,86 @@ def send_velocity_body(velocity_x, velocity_y, velocity_z, yaw_rate=0):
5685
position=(0, 0, 0),
5786
velocity=(velocity_x, velocity_y, velocity_z),
5887
acceleration=(0, 0, 0),
59-
attitude=(0, 0, 0, yaw_rate),
60-
attitude_rate=(0, 0, 0, 0)
88+
attitude=(0, 0, 0, 0),
89+
attitude_rate=(0, 0, 0, yaw_rate)
6190
)
6291
packed_data = packet.pack()
6392
sock.sendto(packed_data, (UDP_IP, UDP_PORT))
64-
print(f"Sent: Velocity=({velocity_x}, {velocity_y}, {velocity_z}) Yaw={yaw_rate}")
93+
94+
def display_text(message, position, font=FONT, color=TEXT_COLOR):
95+
"""Displays text on the Pygame screen at the given position."""
96+
text = font.render(message, True, color)
97+
screen.blit(text, position)
6598

6699
def main():
67-
print("Drone Velocity and Yaw Control Sender Running...")
68-
print("Use W, A, S, D, Arrow keys, and H to control the drone. Press 'Q' to quit.")
69-
70-
vx, vy, vz, yaw_rate = 0, 0, 0, 0
71-
72-
while True:
73-
try:
74-
if keyboard.is_pressed('q'):
75-
print("Quitting...")
76-
break
77-
78-
if keyboard.is_pressed('w'):
79-
vx = DEFAULT_SPEED
80-
elif keyboard.is_pressed('s'):
81-
vx = -DEFAULT_SPEED
82-
elif keyboard.is_pressed('a'):
83-
vy = -DEFAULT_SPEED
84-
elif keyboard.is_pressed('d'):
85-
vy = DEFAULT_SPEED
86-
87-
if keyboard.is_pressed('up'):
88-
vz = DEFAULT_SPEED
89-
elif keyboard.is_pressed('down'):
90-
vz = -DEFAULT_SPEED
91-
92-
if keyboard.is_pressed('left'):
93-
yaw_rate -= DEFAULT_YAW_RATE
94-
elif keyboard.is_pressed('right'):
95-
yaw_rate += DEFAULT_YAW_RATE
96-
97-
if keyboard.is_pressed('h'):
98-
# Hold current velocities and yaw rate
99-
send_velocity_body(vx, vy, vz, yaw_rate)
100-
continue
101-
102-
send_velocity_body(vx, vy, vz, yaw_rate)
103-
104-
except Exception as e:
105-
print(f"Error: {str(e)}")
106-
107-
# Reset velocities and yaw rate after sending to simulate continuous control
108-
vx, vy, vz, yaw_rate = 0, 0, 0, 0
109-
110-
# Ensure the drone stops moving when the script exits
111-
send_velocity_body(0, 0, 0, 0)
100+
"""Main function to handle keyboard inputs for drone control."""
101+
running = True
102+
enabled = False
103+
velocity_x, velocity_y, velocity_z, yaw_rate = 0, 0, 0, 0
104+
clock = pygame.time.Clock()
105+
106+
while running:
107+
screen.fill(BACKGROUND_COLOR)
108+
display_text("MAVSDK Offboard Control: Local Velocity Sender", (50, 20), font=FONT)
109+
display_text("Press 'E' to enable, 'C' to cancel, 'H' to hold, 'Q' to quit", (50, 50), font=SMALL_FONT)
110+
if enabled:
111+
display_text("Status: Enabled", (50, 100), font=SMALL_FONT, color=GREEN)
112+
else:
113+
display_text("Status: Disabled", (50, 100), font=SMALL_FONT, color=RED)
114+
display_text(f"Current Command: Vx={velocity_x}, Vy={velocity_y}, Vz={velocity_z}, Yaw Rate={yaw_rate}", (50, 500), font=SMALL_FONT)
115+
display_text(f"IP: {UDP_IP}, Port: {UDP_PORT}, Rate: {SEND_RATE}s", (50, 550), font=SMALL_FONT)
116+
117+
for event in pygame.event.get():
118+
if event.type == pygame.QUIT:
119+
running = False
120+
elif event.type == pygame.KEYDOWN:
121+
if event.key == pygame.K_q:
122+
send_velocity_body(0, 0, 0, 0) # Safety stop
123+
running = False
124+
elif event.key == pygame.K_e:
125+
enabled = True
126+
elif event.key == pygame.K_c:
127+
send_velocity_body(0, 0, 0, 0) # Safety stop
128+
enabled = False
129+
elif event.key == pygame.K_h:
130+
velocity_x, velocity_y, velocity_z, yaw_rate = 0, 0, 0, 0
131+
132+
if enabled:
133+
if event.key == pygame.K_w:
134+
velocity_x = DEFAULT_SPEED
135+
elif event.key == pygame.K_s:
136+
velocity_x = -DEFAULT_SPEED
137+
elif event.key == pygame.K_a:
138+
velocity_y = -DEFAULT_SPEED
139+
elif event.key == pygame.K_d:
140+
velocity_y = DEFAULT_SPEED
141+
elif event.key == pygame.K_UP:
142+
velocity_z = -DEFAULT_SPEED # Default Down was Up so Reversed
143+
elif event.key == pygame.K_DOWN:
144+
velocity_z = DEFAULT_SPEED # Default Down was Up so Reversed
145+
elif event.key == pygame.K_LEFT:
146+
yaw_rate -= YAW_RATE_STEP
147+
elif event.key == pygame.K_RIGHT:
148+
yaw_rate += YAW_RATE_STEP
149+
150+
elif event.type == pygame.KEYUP:
151+
if event.key in [pygame.K_w, pygame.K_s]:
152+
velocity_x = 0
153+
elif event.key in [pygame.K_a, pygame.K_d]:
154+
velocity_y = 0
155+
elif event.key in [pygame.K_UP, pygame.K_DOWN]:
156+
velocity_z = 0
157+
elif event.key in [pygame.K_LEFT, pygame.K_RIGHT]:
158+
yaw_rate = 0
159+
160+
if enabled:
161+
send_velocity_body(velocity_x, velocity_y, velocity_z, yaw_rate)
162+
163+
pygame.display.flip()
164+
clock.tick(1 / SEND_RATE)
165+
112166
sock.close()
167+
pygame.quit()
113168

114169
if __name__ == "__main__":
115170
main()

‎examples/offboard_from_stream/control_packet.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,7 @@ class SetpointMode(Enum):
8484
class ControlPacket:
8585
HEADER = 0xDEADBEEFDEADBEEF
8686
HEADER_FORMAT = ">Q" # 8 bytes for header
87-
DATA_FORMAT = ">QII3d3d3d4d4dI" # Updated format to include all required fields
87+
DATA_FORMAT = ">QIII3d3d3d4d4d" # Updated format to include all required fields
8888
CRC_FORMAT = ">I" # 4 bytes for CRC
8989

9090
def __init__(self, mode, enable_flag, yaw_control_flag, position, velocity, acceleration, attitude, attitude_rate, timestamp=None):

‎examples/offboard_from_stream/receiver.py

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -34,11 +34,6 @@
3434
handles the specified control setpoints (position, velocity, yaw, etc.) and updates the drone's
3535
state accordingly.
3636
37-
Commands:
38-
- 's' to start listening and processing packets.
39-
- 'p' to pause processing (halts sending control commands to the drone).
40-
- 'r' to resume.
41-
- 'q' to stop listening and cleanly disconnect from the MAVSDK server.
4237
4338
Author:
4439
-------
@@ -137,6 +132,14 @@ async def handle_packet(drone, packet):
137132
print(f"Sending POSITION_LOCAL_NED setpoint: North {control_packet.position[0]}, East {control_packet.position[1]}, Down {control_packet.position[2]}, Yaw {yaw}")
138133
await drone.offboard.set_position_ned(
139134
PositionNedYaw(control_packet.position[0], control_packet.position[1], control_packet.position[2], yaw))
135+
136+
if control_packet.setpoint_flags & SetpointMode.VELOCITY_BODY.value:
137+
vx = control_packet.velocity[0]
138+
vy = control_packet.velocity[1]
139+
vz = control_packet.velocity[2]
140+
yaw_rate = control_packet.attitude_rate[3] # Assume yaw rate is the fourth element in attitude tuple
141+
await drone.offboard.set_velocity_body(VelocityBodyYawspeed(vx, vy, vz, yaw_rate))
142+
print(f"Setting VELOCITY_BODY setpoint: Vx={vx}, Vy={vy}, Vz={vz}, Yaw rate={yaw_rate}")
140143

141144
else:
142145
is_active = await drone.offboard.is_active()

0 commit comments

Comments
 (0)