Skip to content

Commit c789e38

Browse files
julianoesjulianoes
authored
Merge pull request #747 from Pjort/main
Updated to function with the changes of MAVSDK 3.0.0
2 parents 49c0cda + 13f9944 commit c789e38

File tree

1 file changed

+122
-73
lines changed

1 file changed

+122
-73
lines changed

‎examples/gimbal.py

Lines changed: 122 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -2,86 +2,135 @@
22

33
import asyncio
44
from mavsdk import System
5-
from mavsdk.gimbal import GimbalMode, ControlMode
5+
from mavsdk.gimbal import GimbalMode, ControlMode, SendMode
6+
7+
8+
async def get_gimbals(drone, timeout=10):
9+
gimbals_found = [] # List to store all gimbals found
10+
11+
async def fetch_gimbals():
12+
# gimbal_list is a GimbalList object
13+
async for gimbal_list in drone.gimbal.gimbal_list():
14+
# gimbal_list.gimbals contains GimbalItem objects
15+
for gimbal in gimbal_list.gimbals:
16+
print(
17+
f"Found Gimbal: ID={gimbal.gimbal_id}, "
18+
f"Model={gimbal.model_name}, "
19+
f"Vendor={gimbal.vendor_name}, "
20+
f"Device ID={gimbal.gimbal_device_id}"
21+
)
22+
gimbals_found.append(gimbal)
23+
return
24+
25+
try:
26+
# Apply timeout to async loop
27+
await asyncio.wait_for(fetch_gimbals(), timeout=timeout)
28+
return gimbals_found
29+
except asyncio.TimeoutError:
30+
print("Timeout: No gimbals found.")
631

732

833
async def run():
934
# Init the drone
1035
drone = System()
1136
await drone.connect(system_address="udp://:14540")
1237

13-
# Start printing gimbal position updates
14-
print_gimbal_position_task = \
15-
asyncio.ensure_future(print_gimbal_position(drone))
16-
17-
print("Taking control of gimbal")
18-
await drone.gimbal.take_control(ControlMode.PRIMARY)
19-
20-
# Set the gimbal to YAW_LOCK (= 1) mode (see docs for the difference)
21-
# Other valid values: YAW_FOLLOW (= 0)
22-
# YAW_LOCK will fix the gimbal pointing to an absolute direction,
23-
# whereas YAW_FOLLOW will point relative to vehicle heading.
24-
print("Setting gimbal mode")
25-
await drone.gimbal.set_mode(GimbalMode.YAW_FOLLOW)
26-
27-
print("Look forward first")
28-
await drone.gimbal.set_pitch_and_yaw(0, 0)
29-
await asyncio.sleep(1)
30-
31-
print("Look down")
32-
await drone.gimbal.set_pitch_and_yaw(-90, 0)
33-
await asyncio.sleep(2)
34-
35-
print("Back to horizontal")
36-
await drone.gimbal.set_pitch_and_yaw(0, 0)
37-
await asyncio.sleep(2)
38-
39-
print("Slowly look up")
40-
await drone.gimbal.set_pitch_rate_and_yaw_rate(10, 0)
41-
await asyncio.sleep(3)
42-
43-
print("Back to horizontal")
44-
await drone.gimbal.set_pitch_and_yaw(0, 0)
45-
await asyncio.sleep(2)
46-
47-
print("Look right")
48-
await drone.gimbal.set_pitch_and_yaw(0, 90)
49-
await asyncio.sleep(2)
50-
51-
print("Look forward again")
52-
await drone.gimbal.set_pitch_and_yaw(0, 0)
53-
await asyncio.sleep(2)
54-
55-
print("Slowly look to the left")
56-
await drone.gimbal.set_pitch_rate_and_yaw_rate(0, -20)
57-
await asyncio.sleep(3)
58-
59-
print("Look forward again")
60-
await drone.gimbal.set_pitch_and_yaw(0, 0)
61-
await asyncio.sleep(2)
62-
63-
# Set the gimbal to track a region of interest (lat, lon, altitude)
64-
# Units are degrees and meters MSL respectively
65-
print("Look at a ROI (region of interest)")
66-
await drone.gimbal.set_roi_location(47.39743832, 8.5463316, 488)
67-
await asyncio.sleep(3)
68-
69-
print("Look forward again")
70-
await drone.gimbal.set_pitch_and_yaw(0, 0)
71-
await asyncio.sleep(2)
72-
73-
print("Release control of gimbal again")
74-
await drone.gimbal.release_control()
75-
76-
print_gimbal_position_task.cancel()
77-
78-
79-
async def print_gimbal_position(drone):
80-
# Report gimbal position updates asynchronously
81-
# Note that we are getting gimbal position updates in
82-
# euler angles; we can also get them as quaternions
83-
async for angle in drone.telemetry.camera_attitude_euler():
84-
print(f"Gimbal pitch: {angle.pitch_deg}, yaw: {angle.yaw_deg}")
38+
gimbals = await get_gimbals(drone)
39+
40+
for gimbal in gimbals:
41+
42+
# Start printing gimbal position updates
43+
print_gimbal_position_task = asyncio.create_task(
44+
print_gimbal_attitude(gimbal.gimbal_id, drone)
45+
)
46+
47+
print("Taking control of gimbal with ID", gimbal.gimbal_id)
48+
await drone.gimbal.take_control(gimbal.gimbal_id, ControlMode.PRIMARY)
49+
50+
print("Look forward with gimbal ID", gimbal.gimbal_id)
51+
await drone.gimbal.set_angles(
52+
gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE
53+
)
54+
await asyncio.sleep(2)
55+
56+
print("Look down with gimbal ID", gimbal.gimbal_id)
57+
await drone.gimbal.set_angles(
58+
gimbal.gimbal_id, 0, -90, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE
59+
)
60+
await asyncio.sleep(4)
61+
62+
print("Back to horizontal with gimbal ID", gimbal.gimbal_id)
63+
await drone.gimbal.set_angles(
64+
gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE
65+
)
66+
await asyncio.sleep(4)
67+
68+
print("Slowly look up with gimbal ID", gimbal.gimbal_id)
69+
await drone.gimbal.set_angular_rates(
70+
gimbal.gimbal_id, 0, 5, 0, GimbalMode.YAW_FOLLOW, SendMode.STREAM
71+
)
72+
await asyncio.sleep(4)
73+
74+
print("Back to horizontal with gimbal ID", gimbal.gimbal_id)
75+
await drone.gimbal.set_angles(
76+
gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE
77+
)
78+
await asyncio.sleep(4)
79+
80+
print("Look right with gimbal ID", gimbal.gimbal_id)
81+
await drone.gimbal.set_angles(
82+
gimbal.gimbal_id, 0, 0, 90, GimbalMode.YAW_FOLLOW, SendMode.ONCE
83+
)
84+
await asyncio.sleep(4)
85+
86+
print("Look forward again with gimbal ID", gimbal.gimbal_id)
87+
await drone.gimbal.set_angles(
88+
gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE
89+
)
90+
await asyncio.sleep(4)
91+
92+
print("Slowly look to the left with gimbal ID", gimbal.gimbal_id)
93+
await drone.gimbal.set_angular_rates(
94+
gimbal.gimbal_id, 0, 0, -5, GimbalMode.YAW_FOLLOW, SendMode.STREAM
95+
)
96+
await asyncio.sleep(4)
97+
98+
print("Look forward again with gimbal ID", gimbal.gimbal_id)
99+
await drone.gimbal.set_angles(
100+
gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE
101+
)
102+
await asyncio.sleep(4)
103+
104+
# Set the gimbal to track a region of interest (lat, lon, altitude)
105+
# Units are degrees and meters MSL respectively
106+
print(
107+
"Look at a ROI (region of interest) with gimbal ID",
108+
gimbal.gimbal_id)
109+
await drone.gimbal.set_roi_location(
110+
gimbal.gimbal_id, 47.39743832, 8.5463316, 488
111+
)
112+
await asyncio.sleep(4)
113+
114+
print("Back to forward again with gimbal ID", gimbal.gimbal_id)
115+
await drone.gimbal.set_angles(
116+
gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE
117+
)
118+
await asyncio.sleep(4)
119+
120+
print("Release control of gimbal with ID", gimbal.gimbal_id)
121+
await drone.gimbal.release_control(gimbal.gimbal_id)
122+
123+
print_gimbal_position_task.cancel()
124+
125+
126+
async def print_gimbal_attitude(gimbal_id, drone):
127+
while True:
128+
attitude = await drone.gimbal.get_attitude(gimbal_id)
129+
print(
130+
f"Gimbal ID {gimbal_id} "
131+
f"pitch: {attitude.euler_angle_forward.pitch_deg}, "
132+
f"yaw: {attitude.euler_angle_forward.yaw_deg}")
133+
await asyncio.sleep(0.5)
85134

86135

87136
if __name__ == "__main__":

0 commit comments

Comments
 (0)