From 00bc7fcea8b098a00103f517e61cd7f89488ebd4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 4 Mar 2021 17:28:59 +0100
Subject: [PATCH] examples: improve gimbal example

This adds a few more demo steps, as well as takes and releases control.
---
 examples/gimbal.py | 65 ++++++++++++++++++++++++++++++++++------------
 1 file changed, 48 insertions(+), 17 deletions(-)

diff --git a/examples/gimbal.py b/examples/gimbal.py
index bc06d4bc..11070c76 100755
--- a/examples/gimbal.py
+++ b/examples/gimbal.py
@@ -2,7 +2,7 @@
 
 import asyncio
 from mavsdk import System
-from mavsdk.gimbal import GimbalMode
+from mavsdk.gimbal import GimbalMode, ControlMode
 
 async def run():
     # Init the drone
@@ -12,41 +12,72 @@ async def run():
     # Start printing gimbal position updates
     asyncio.ensure_future(print_gimbal_position(drone))
 
-    # Arm and takeoff the drone
-    await drone.action.arm()
-    await drone.action.takeoff()
+    print("Taking control of gimbal")
+    await drone.gimbal.take_control(ControlMode.PRIMARY)
 
     # Set the gimbal to YAW_LOCK (= 1) mode (see docs for the difference)
     # Other valid values: YAW_FOLLOW (= 0)
     # YAW_LOCK will fix the gimbal pointing to an absolute direction,
     # whereas YAW_FOLLOW will point relative to vehicle heading.
     print("Setting gimbal mode")
-    await drone.gimbal.set_mode(GimbalMode.YAW_LOCK)
-    await asyncio.sleep(5)
+    await drone.gimbal.set_mode(GimbalMode.YAW_FOLLOW)
 
-    # Move the gimbal to point at pitch -40 degrees, yaw 30 degrees
-    print("Setting pitch & yaw")
-    await drone.gimbal.set_pitch_and_yaw(-40, 30)
-    await asyncio.sleep(10)
+    print("Look forward first")
+    await drone.gimbal.set_pitch_and_yaw(0, 0)
+    await asyncio.sleep(1)
 
+    print("Look down")
+    await drone.gimbal.set_pitch_and_yaw(-90, 0)
+    await asyncio.sleep(2)
+
+    print("Back to horizontal")
+    await drone.gimbal.set_pitch_and_yaw(0, 0)
+    await asyncio.sleep(2)
+
+    print("Slowly look up")
+    await drone.gimbal.set_pitch_rate_and_yaw_rate(10, 0)
+    await asyncio.sleep(3)
+
+    print("Back to horizontal")
+    await drone.gimbal.set_pitch_and_yaw(0, 0)
+    await asyncio.sleep(2)
+
+    print("Look right")
+    await drone.gimbal.set_pitch_and_yaw(0, 90)
+    await asyncio.sleep(2)
+
+    print("Look forward again")
+    await drone.gimbal.set_pitch_and_yaw(0, 0)
+    await asyncio.sleep(2)
+
+    print("Slowly look to the left")
+    await drone.gimbal.set_pitch_rate_and_yaw_rate(0, -20)
+    await asyncio.sleep(3)
+
+    print("Look forward again")
+    await drone.gimbal.set_pitch_and_yaw(0, 0)
+    await asyncio.sleep(2)
 
     # Set the gimbal to track a region of interest (lat, lon, altitude)
     # Units are degrees and meters MSL respectively
-    print("Setting RoI")
-    await drone.gimbal.set_roi_location(28.452386, -13.867138, 28.5)
-    await asyncio.sleep(10)
+    print("Look at a ROI (region of interest)")
+    await drone.gimbal.set_roi_location(47.39743832, 8.5463316, 488)
+    await asyncio.sleep(3)
 
-    await drone.action.land()
-    await asyncio.sleep(5)
+    print("Look forward again")
+    await drone.gimbal.set_pitch_and_yaw(0, 0)
+    await asyncio.sleep(2)
 
+    print("Release control of gimbal again")
+    await drone.gimbal.release_control()
 
 
 async def print_gimbal_position(drone):
     # Report gimbal position updates asynchronously
     # Note that we are getting gimbal position updates in
     # euler angles; we can also get them as quaternions
-    async for position in drone.telemetry.camera_attitude_euler():
-        print(position)
+    async for angle in drone.telemetry.camera_attitude_euler():
+        print(f"Gimbal pitch: {angle.pitch_deg}, yaw: {angle.yaw_deg}")
 
 
 if __name__ == "__main__":