Skip to content

Commit 61dc71d

Browse files
PjortPjort
committed
Updated to function with the changes of MAVSDK 3.0.0
1 parent 49c0cda commit 61dc71d

File tree

1 file changed

+67
-57
lines changed

1 file changed

+67
-57
lines changed

‎examples/gimbal.py

Lines changed: 67 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -2,88 +2,98 @@
22

33
import asyncio
44
from mavsdk import System
5-
from mavsdk.gimbal import GimbalMode, ControlMode
5+
from mavsdk.gimbal import GimbalMode, ControlMode, SendMode
66

77

8+
async def get_gimbals(drone, timeout=10):
9+
gimbals_found = [] # List to store all gimbals found
10+
11+
async def fetch_gimbals():
12+
async for gimbal_list in drone.gimbal.gimbal_list(): # gimbal_list is a GimbalList object
13+
for gimbal in gimbal_list.gimbals: # gimbal_list.gimbals contains GimbalItem objects
14+
print(f"Found Gimbal: ID={gimbal.gimbal_id}, Model={gimbal.model_name}, Vendor={gimbal.vendor_name}, Device ID={gimbal.gimbal_device_id}")
15+
gimbals_found.append(gimbal)
16+
return
17+
18+
try:
19+
await asyncio.wait_for(fetch_gimbals(), timeout=timeout) # Apply timeout to async loop
20+
return gimbals_found
21+
except asyncio.TimeoutError:
22+
print("Timeout: No gimbals found.")
23+
824
async def run():
925
# Init the drone
1026
drone = System()
1127
await drone.connect(system_address="udp://:14540")
1228

13-
# Start printing gimbal position updates
14-
print_gimbal_position_task = \
15-
asyncio.ensure_future(print_gimbal_position(drone))
29+
gimbals = await get_gimbals(drone)
1630

17-
print("Taking control of gimbal")
18-
await drone.gimbal.take_control(ControlMode.PRIMARY)
31+
for gimbal in gimbals:
1932

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)
33+
# Start printing gimbal position updates
34+
print_gimbal_position_task = asyncio.create_task(print_gimbal_attitude(gimbal.gimbal_id, drone))
2635

27-
print("Look forward first")
28-
await drone.gimbal.set_pitch_and_yaw(0, 0)
29-
await asyncio.sleep(1)
36+
print("Taking control of gimbal with ID", gimbal.gimbal_id)
37+
await drone.gimbal.take_control(gimbal.gimbal_id, ControlMode.PRIMARY)
3038

31-
print("Look down")
32-
await drone.gimbal.set_pitch_and_yaw(-90, 0)
33-
await asyncio.sleep(2)
39+
print("Look forward with gimbal ID", gimbal.gimbal_id)
40+
await drone.gimbal.set_angles(gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE)
41+
await asyncio.sleep(2)
3442

35-
print("Back to horizontal")
36-
await drone.gimbal.set_pitch_and_yaw(0, 0)
37-
await asyncio.sleep(2)
43+
print("Look down with gimbal ID", gimbal.gimbal_id)
44+
await drone.gimbal.set_angles(gimbal.gimbal_id, 0, -90, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE)
45+
await asyncio.sleep(4)
3846

39-
print("Slowly look up")
40-
await drone.gimbal.set_pitch_rate_and_yaw_rate(10, 0)
41-
await asyncio.sleep(3)
47+
print("Back to horizontal with gimbal ID", gimbal.gimbal_id)
48+
await drone.gimbal.set_angles(gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE)
49+
await asyncio.sleep(4)
4250

43-
print("Back to horizontal")
44-
await drone.gimbal.set_pitch_and_yaw(0, 0)
45-
await asyncio.sleep(2)
51+
print("Slowly look up with gimbal ID", gimbal.gimbal_id)
52+
await drone.gimbal.set_angular_rates(gimbal.gimbal_id, 0, 5, 0, GimbalMode.YAW_FOLLOW, SendMode.STREAM)
53+
await asyncio.sleep(4)
4654

47-
print("Look right")
48-
await drone.gimbal.set_pitch_and_yaw(0, 90)
49-
await asyncio.sleep(2)
55+
print("Back to horizontal with gimbal ID", gimbal.gimbal_id)
56+
await drone.gimbal.set_angles(gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE)
57+
await asyncio.sleep(4)
5058

51-
print("Look forward again")
52-
await drone.gimbal.set_pitch_and_yaw(0, 0)
53-
await asyncio.sleep(2)
59+
print("Look right with gimbal ID", gimbal.gimbal_id)
60+
await drone.gimbal.set_angles(gimbal.gimbal_id, 0, 0, 90, GimbalMode.YAW_FOLLOW, SendMode.ONCE)
61+
await asyncio.sleep(4)
5462

55-
print("Slowly look to the left")
56-
await drone.gimbal.set_pitch_rate_and_yaw_rate(0, -20)
57-
await asyncio.sleep(3)
63+
print("Look forward again with gimbal ID", gimbal.gimbal_id)
64+
await drone.gimbal.set_angles(gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE)
65+
await asyncio.sleep(4)
5866

59-
print("Look forward again")
60-
await drone.gimbal.set_pitch_and_yaw(0, 0)
61-
await asyncio.sleep(2)
67+
print("Slowly look to the left with gimbal ID", gimbal.gimbal_id)
68+
await drone.gimbal.set_angular_rates(gimbal.gimbal_id, 0, 0, -5, GimbalMode.YAW_FOLLOW, SendMode.STREAM)
69+
await asyncio.sleep(4)
6270

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)
71+
print("Look forward again with gimbal ID", gimbal.gimbal_id)
72+
await drone.gimbal.set_angles(gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE)
73+
await asyncio.sleep(4)
6874

69-
print("Look forward again")
70-
await drone.gimbal.set_pitch_and_yaw(0, 0)
71-
await asyncio.sleep(2)
75+
# Set the gimbal to track a region of interest (lat, lon, altitude)
76+
# Units are degrees and meters MSL respectively
77+
print("Look at a ROI (region of interest) with gimbal ID", gimbal.gimbal_id)
78+
await drone.gimbal.set_roi_location(gimbal.gimbal_id, 47.39743832, 8.5463316, 488)
79+
await asyncio.sleep(4)
7280

73-
print("Release control of gimbal again")
74-
await drone.gimbal.release_control()
81+
print("Back to forward again with gimbal ID", gimbal.gimbal_id)
82+
await drone.gimbal.set_angles(gimbal.gimbal_id, 0, 0, 0, GimbalMode.YAW_FOLLOW, SendMode.ONCE)
83+
await asyncio.sleep(4)
7584

76-
print_gimbal_position_task.cancel()
85+
print("Release control of gimbal with ID", gimbal.gimbal_id)
86+
await drone.gimbal.release_control(gimbal.gimbal_id)
7787

88+
print_gimbal_position_task.cancel()
7889

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}")
90+
async def print_gimbal_attitude(gimbal_id, drone):
91+
while True:
92+
attitude = await drone.gimbal.get_attitude(gimbal_id)
93+
print(f"Gimbal ID {gimbal_id} pitch: {attitude.euler_angle_forward.pitch_deg}, yaw: {attitude.euler_angle_forward.yaw_deg}")
94+
await asyncio.sleep(0.5)
8595

8696

8797
if __name__ == "__main__":
8898
# Run the asyncio loop
89-
asyncio.run(run())
99+
asyncio.run(run())

0 commit comments

Comments
 (0)