Skip to content

Commit c1d5556

Browse files
alireza787balireza787b
committed
added attitude setpoint
1 parent 61a068c commit c1d5556

File tree

3 files changed

+248
-41
lines changed

3 files changed

+248
-41
lines changed

‎examples/offboard_from_stream/attitude_sender_example.py

Lines changed: 186 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,186 @@
1+
"""
2+
MAVSDK Offboard Control - Attitude Control Sender
3+
=================================================
4+
5+
This script provides an interface for controlling a drone's attitude (roll, pitch, yaw) and thrust
6+
through keyboard inputs, utilizing MAVSDK over UDP. It features an interactive GUI built with Pygame
7+
for real-time control and feedback, enabling dynamic adjustment of the drone's flight parameters.
8+
9+
Overview:
10+
---------
11+
- Sends control packets to command drone attitudes and thrust in local body coordinates.
12+
- Offers two modes of operation: 'Instant Reset' and 'Incremental Control', toggled by pressing 'M'.
13+
- Provides a graphical interface to visualize and control the drone's orientation and thrust.
14+
15+
Setup Requirements:
16+
-------------------
17+
- A MAVSDK-compatible drone or a SITL setup running and accessible on the network.
18+
- The receiver node (`receiver.py`) must be operational to handle and execute the commands sent from this script.
19+
- Ensure that the receiver and this sender script are configured to communicate over the specified IP and port.
20+
21+
Key Functionalities:
22+
--------------------
23+
- **Attitude Control**: Use W, S, A, D for adjusting pitch and roll.
24+
- W: Decrease pitch (nose down)
25+
- S: Increase pitch (nose up)
26+
- A: Decrease roll (left down)
27+
- D: Increase roll (right down)
28+
- **Thrust Adjustment**: Up and Down arrow keys adjust thrust.
29+
- **Yaw Control**: Left and Right arrow keys adjust yaw.
30+
- **Mode Switching**: Press 'M' to toggle between 'Instant Reset' and 'Incremental Control' modes.
31+
- **Control Enable/Disable**: 'E' to enable sending commands, 'C' to cancel and send a stop command.
32+
- **Emergency Hold**: Press 'H' to immediately hold the current attitude and thrust, effectively stopping any adjustments.
33+
- **Application Exit**: Press 'Q' to safely exit the application, ensuring all movements are halted.
34+
35+
Usage Instructions:
36+
-------------------
37+
1. Ensure your MAVSDK setup (either SITL or a real drone) is operational and that `receiver.py` is running.
38+
2. Start this script in a Python environment where Pygame is installed. The script's GUI will display on your screen.
39+
3. Use the keyboard controls as outlined to command the drone. Ensure you start command transmission by pressing 'E' and can stop it anytime with 'H' or 'C'.
40+
41+
Safety Notice:
42+
--------------
43+
- When operating with a real drone, ensure you are in a safe, open environment to avoid any accidents.
44+
- Always be prepared to take manual control of the drone if necessary.
45+
46+
Author:
47+
- Alireza Ghaderi
48+
- GitHub: alireza787b
49+
- Date: April 2024
50+
51+
Dependencies:
52+
- Pygame for GUI operations.
53+
- MAVSDK for drone control interfacing.
54+
- Python's `socket` library for UDP communication.
55+
- `control_packet.py` for formatting control commands.
56+
57+
The code is designed to be clear and modifiable for different use cases, allowing adjustments to IP settings, control rates, and more directly within the script.
58+
"""
59+
import socket
60+
import pygame
61+
import sys
62+
from control_packet import ControlPacket, SetpointMode
63+
64+
# Constants for communication and control
65+
UDP_IP = "127.0.0.1"
66+
UDP_PORT = 5005
67+
SEND_RATE = 0.1 # Packet send rate in seconds (10 Hz)
68+
ROLL_PITCH_STEP = 2.0 # degrees step for roll and pitch
69+
YAW_RATE_STEP = 5.0 # degrees step for yaw
70+
THRUST_STEP = 0.05 # thrust step
71+
INCREMENTAL_MODE = False # False for instant reset, True for incremental control
72+
73+
# Initialize Pygame and set up the display
74+
pygame.init()
75+
screen = pygame.display.set_mode((800, 600))
76+
pygame.display.set_caption('MAVSDK Offboard Control - Attitude Control')
77+
78+
# Colors, fonts, and initial settings
79+
BACKGROUND_COLOR = (30, 30, 30)
80+
TEXT_COLOR = (255, 255, 255)
81+
FONT = pygame.font.Font(None, 36)
82+
SMALL_FONT = pygame.font.Font(None, 24)
83+
GREEN = (0, 255, 0)
84+
RED = (255, 0, 0)
85+
86+
# Setup UDP socket for sending commands
87+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
88+
89+
def send_attitude(roll, pitch, yaw, thrust):
90+
"""Send an attitude command to the drone."""
91+
packet = ControlPacket(
92+
mode=SetpointMode.ATTITUDE_CONTROL,
93+
enable_flag=True,
94+
yaw_control_flag=True,
95+
position=(0, 0, 0), # Not used in attitude mode
96+
velocity=(0, 0, 0), # Not used in attitude mode
97+
acceleration=(0, 0, 0), # Not used in attitude mode
98+
attitude=(roll, pitch, yaw, thrust),
99+
attitude_rate=(0, 0, 0, 0)
100+
)
101+
packed_data = packet.pack()
102+
sock.sendto(packed_data, (UDP_IP, UDP_PORT))
103+
104+
def display_text(message, position, font=FONT, color=TEXT_COLOR):
105+
"""Displays text on the Pygame screen at the given position."""
106+
text = font.render(message, True, color)
107+
screen.blit(text, position)
108+
109+
def main():
110+
"""Main function to handle keyboard inputs for drone attitude control."""
111+
global INCREMENTAL_MODE
112+
running = True
113+
enabled = False
114+
roll, pitch, yaw, thrust = 0, 0, 0, 0.5 # Start with a neutral thrust value
115+
clock = pygame.time.Clock()
116+
117+
while running:
118+
screen.fill(BACKGROUND_COLOR)
119+
display_text("MAVSDK Offboard Control: Attitude Control", (50, 20), font=FONT)
120+
display_text("Press 'E' to enable, 'C' to cancel, 'M' to toggle mode, 'H' to hold, 'Q' to quit", (50, 50), font=SMALL_FONT)
121+
mode_text = "Incremental" if INCREMENTAL_MODE else "Instant Reset"
122+
display_text(f"Mode: {mode_text}", (50, 80), font=SMALL_FONT)
123+
if enabled:
124+
display_text("Status: Enabled", (50, 100), font=SMALL_FONT, color=GREEN)
125+
else:
126+
display_text("Status: Disabled", (50, 100), font=SMALL_FONT, color=RED)
127+
display_text(f"Current Command: Roll={roll}, Pitch={pitch}, Yaw={yaw}, Thrust={thrust}", (50, 500), font=SMALL_FONT)
128+
display_text(f"IP: {UDP_IP}, Port: {UDP_PORT}, Rate: {SEND_RATE}s", (50, 550), font=SMALL_FONT)
129+
130+
for event in pygame.event.get():
131+
if event.type == pygame.QUIT:
132+
running = False
133+
elif event.type == pygame.KEYDOWN:
134+
if event.key == pygame.K_q:
135+
send_attitude(0, 0, 0, 0) # Safety stop
136+
running = False
137+
elif event.key == pygame.K_e:
138+
enabled = True
139+
elif event.key == pygame.K_c:
140+
send_attitude(0, 0, 0, 0) # Safety stop
141+
enabled = False
142+
elif event.key == pygame.K_m:
143+
INCREMENTAL_MODE = not INCREMENTAL_MODE
144+
elif event.key == pygame.K_h:
145+
roll, pitch, yaw, thrust = 0, 0, 0, 0.5 # Reset to neutral thrust
146+
147+
if enabled:
148+
if event.key == pygame.K_w:
149+
pitch -= ROLL_PITCH_STEP if INCREMENTAL_MODE else -ROLL_PITCH_STEP
150+
elif event.key == pygame.K_s:
151+
pitch += ROLL_PITCH_STEP if INCREMENTAL_MODE else ROLL_PITCH_STEP
152+
elif event.key == pygame.K_a:
153+
roll -= ROLL_PITCH_STEP if INCREMENTAL_MODE else -ROLL_PITCH_STEP
154+
elif event.key == pygame.K_d:
155+
roll += ROLL_PITCH_STEP if INCREMENTAL_MODE else ROLL_PITCH_STEP
156+
elif event.key == pygame.K_UP:
157+
thrust = min(thrust + THRUST_STEP, 1.0) # Ensure thrust does not exceed 1
158+
elif event.key == pygame.K_DOWN:
159+
thrust = max(thrust - THRUST_STEP, 0) # Ensure thrust does not go below 0
160+
elif event.key == pygame.K_LEFT:
161+
yaw -= YAW_RATE_STEP if INCREMENTAL_MODE else -YAW_RATE_STEP
162+
elif event.key == pygame.K_RIGHT:
163+
yaw += YAW_RATE_STEP if INCREMENTAL_MODE else YAW_RATE_STEP
164+
165+
elif event.type == pygame.KEYUP:
166+
if not INCREMENTAL_MODE:
167+
if event.key in [pygame.K_w, pygame.K_s]:
168+
pitch = 0
169+
elif event.key in [pygame.K_a, pygame.K_d]:
170+
roll = 0
171+
elif event.key in [pygame.K_UP, pygame.K_DOWN]:
172+
thrust = 0.5 # Reset to mid thrust
173+
elif event.key in [pygame.K_LEFT, pygame.K_RIGHT]:
174+
yaw = 0
175+
176+
if enabled:
177+
send_attitude(roll, pitch, yaw, thrust)
178+
179+
pygame.display.flip()
180+
clock.tick(1 / SEND_RATE)
181+
182+
sock.close()
183+
pygame.quit()
184+
185+
if __name__ == "__main__":
186+
main()

‎examples/offboard_from_stream/body_velocity_sender_example.py

Lines changed: 53 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -2,50 +2,56 @@
22
MAVSDK Offboard Control - Local Velocity Sender
33
=================================================
44
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.
5+
This script provides an interface for controlling a drone's velocity and yaw through keyboard inputs,
6+
utilizing MAVSDK over UDP. It features an interactive GUI built with Pygame for real-time control and feedback,
7+
enabling dynamic adjustment of the drone's flight parameters.
68
79
Overview:
810
---------
911
- 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+
- Offers two modes of operation: 'Instant Reset' and 'Incremental Control', toggled by pressing 'M'.
13+
- Provides a graphical interface to visualize and control the drone's movement and yaw.
1214
1315
Setup Requirements:
1416
-------------------
1517
- A MAVSDK-compatible drone or a SITL setup running and accessible on the network.
1618
- The receiver node (`receiver.py`) must be operational to handle and execute the commands sent from this script.
1719
- Ensure that the receiver and this sender script are configured to communicate over the specified IP and port.
1820
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.
21+
Features:
22+
---------
23+
- **Velocity Control**: Use W, A, S, D for forward, backward, left, and right movements respectively.
24+
- **Altitude Adjustment**: Up and Down arrow keys control vertical movement, with up increasing altitude and down decreasing it.
25+
- **Yaw Control**: Left and Right arrow keys adjust yaw. In 'Instant Reset' mode, yaw rate returns to zero upon key release. In 'Incremental Control' mode, yaw rate persists and increments with each key press.
26+
- **Mode Switching**: Press 'M' to toggle between 'Instant Reset' and 'Incremental Control' modes.
27+
- **Control Enable/Disable**: 'E' to enable sending commands, 'C' to cancel and send a stop command.
28+
- **Emergency Hold**: Press 'H' to immediately stop all movements.
29+
- **Application Exit**: Press 'Q' to safely exit the application, ensuring all movements are halted.
30+
31+
Usage Instructions:
32+
-------------------
33+
1. Ensure your MAVSDK setup (either SITL or a real drone) is operational and that `receiver.py` is running.
34+
2. Start this script in a Python environment where Pygame is installed. The script's GUI will display on your screen.
35+
3. Use the keyboard controls as outlined to command the drone. Ensure you start command transmission by pressing 'E' and can stop it anytime with 'H' or 'C'.
3436
3537
Safety Notice:
3638
--------------
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.
39+
- When operating with a real drone, ensure you are in a safe, open environment to avoid any accidents.
40+
- Always be prepared to take manual control of the drone if necessary.
3941
40-
Author:
42+
Developer:
4143
- Alireza Ghaderi
4244
- GitHub: alireza787b
4345
- Date: April 2024
4446
4547
Dependencies:
4648
- Pygame for GUI operations.
49+
- MAVSDK
50+
- PX4
4751
- Python's `socket` library for UDP communication.
4852
- `control_packet.py` for formatting control commands.
53+
54+
The code is designed to be clear and modifiable for different use cases, allowing adjustments to IP settings, control rates, and more directly within the script.
4955
"""
5056

5157
import socket
@@ -58,7 +64,8 @@
5864
UDP_PORT = 5005
5965
SEND_RATE = 0.1 # Packet send rate in seconds (10 Hz)
6066
DEFAULT_SPEED = 1.0 # meters per second
61-
YAW_RATE_STEP = 10.0 # degrees per step
67+
YAW_RATE_STEP = 5.0 # degrees per step
68+
INCREMENTAL_MODE = False # False for instant reset, True for incremental control
6269

6370
# Initialize Pygame and set up the display
6471
pygame.init()
@@ -98,6 +105,7 @@ def display_text(message, position, font=FONT, color=TEXT_COLOR):
98105

99106
def main():
100107
"""Main function to handle keyboard inputs for drone control."""
108+
global INCREMENTAL_MODE
101109
running = True
102110
enabled = False
103111
velocity_x, velocity_y, velocity_z, yaw_rate = 0, 0, 0, 0
@@ -106,7 +114,9 @@ def main():
106114
while running:
107115
screen.fill(BACKGROUND_COLOR)
108116
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)
117+
display_text("Press 'E' to enable, 'C' to cancel, 'M' to toggle mode, 'H' to hold, 'Q' to quit", (50, 50), font=SMALL_FONT)
118+
mode_text = "Incremental" if INCREMENTAL_MODE else "Instant Reset"
119+
display_text(f"Mode: {mode_text}", (50, 80), font=SMALL_FONT)
110120
if enabled:
111121
display_text("Status: Enabled", (50, 100), font=SMALL_FONT, color=GREEN)
112122
else:
@@ -126,36 +136,39 @@ def main():
126136
elif event.key == pygame.K_c:
127137
send_velocity_body(0, 0, 0, 0) # Safety stop
128138
enabled = False
139+
elif event.key == pygame.K_m:
140+
INCREMENTAL_MODE = not INCREMENTAL_MODE
129141
elif event.key == pygame.K_h:
130142
velocity_x, velocity_y, velocity_z, yaw_rate = 0, 0, 0, 0
131143

132144
if enabled:
133145
if event.key == pygame.K_w:
134-
velocity_x = DEFAULT_SPEED
146+
velocity_x = velocity_x + DEFAULT_SPEED if INCREMENTAL_MODE else DEFAULT_SPEED
135147
elif event.key == pygame.K_s:
136-
velocity_x = -DEFAULT_SPEED
148+
velocity_x = velocity_x - DEFAULT_SPEED if INCREMENTAL_MODE else -DEFAULT_SPEED
137149
elif event.key == pygame.K_a:
138-
velocity_y = -DEFAULT_SPEED
150+
velocity_y = velocity_y - DEFAULT_SPEED if INCREMENTAL_MODE else -DEFAULT_SPEED
139151
elif event.key == pygame.K_d:
140-
velocity_y = DEFAULT_SPEED
152+
velocity_y = velocity_y + DEFAULT_SPEED if INCREMENTAL_MODE else DEFAULT_SPEED
141153
elif event.key == pygame.K_UP:
142-
velocity_z = -DEFAULT_SPEED # Default Down was Up so Reversed
154+
velocity_z = velocity_z - DEFAULT_SPEED if INCREMENTAL_MODE else -DEFAULT_SPEED # Reversed
143155
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
156+
velocity_z = velocity_z + DEFAULT_SPEED if INCREMENTAL_MODE else DEFAULT_SPEED # Reversed
157+
elif event.key == pygame.K_LEFT:
158+
yaw_rate = -YAW_RATE_STEP if not INCREMENTAL_MODE else (yaw_rate - YAW_RATE_STEP)
147159
elif event.key == pygame.K_RIGHT:
148-
yaw_rate += YAW_RATE_STEP
160+
yaw_rate = YAW_RATE_STEP if not INCREMENTAL_MODE else (yaw_rate + YAW_RATE_STEP)
149161

150162
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
163+
if not INCREMENTAL_MODE:
164+
if event.key in [pygame.K_w, pygame.K_s]:
165+
velocity_x = 0
166+
elif event.key in [pygame.K_a, pygame.K_d]:
167+
velocity_y = 0
168+
elif event.key in [pygame.K_UP, pygame.K_DOWN]:
169+
velocity_z = 0
170+
elif event.key in [pygame.K_LEFT, pygame.K_RIGHT]:
171+
yaw_rate = 0
159172

160173
if enabled:
161174
send_velocity_body(velocity_x, velocity_y, velocity_z, yaw_rate)

‎examples/offboard_from_stream/receiver.py

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ async def main():
6969
import asyncio
7070
import socket
7171
from mavsdk import System
72-
from mavsdk.offboard import AccelerationNed, OffboardError, PositionNedYaw, VelocityBodyYawspeed, VelocityNedYaw
72+
from mavsdk.offboard import AccelerationNed, Attitude, OffboardError, PositionNedYaw, VelocityBodyYawspeed, VelocityNedYaw
7373
from control_packet import ControlPacket, SetpointMode # Ensure this import matches your setup
7474

7575
UDP_IP = "127.0.0.1"
@@ -140,6 +140,14 @@ async def handle_packet(drone, packet):
140140
yaw_rate = control_packet.attitude_rate[3] # Assume yaw rate is the fourth element in attitude tuple
141141
await drone.offboard.set_velocity_body(VelocityBodyYawspeed(vx, vy, vz, yaw_rate))
142142
print(f"Setting VELOCITY_BODY setpoint: Vx={vx}, Vy={vy}, Vz={vz}, Yaw rate={yaw_rate}")
143+
144+
if control_packet.setpoint_flags & SetpointMode.ATTITUDE_CONTROL.value:
145+
roll = control_packet.attitude[0]
146+
pitch = control_packet.attitude[1]
147+
yaw = control_packet.attitude[2]
148+
thrust = control_packet.attitude[3] # Ensure thrust is a value between 0 and 1
149+
print(f"Setting ATTITUDE setpoint: Roll={roll}°, Pitch={pitch}°, Yaw={yaw}°, Thrust={thrust}")
150+
await drone.offboard.set_attitude(Attitude(roll, pitch, yaw, thrust))
143151

144152
else:
145153
is_active = await drone.offboard.is_active()

0 commit comments

Comments
 (0)