From 2c9a9468ea8466170b215ef6e9d60ff99b6bb756 Mon Sep 17 00:00:00 2001
From: RexBerry <59031902+RexBerry@users.noreply.github.com>
Date: Tue, 25 Oct 2022 20:03:13 -0500
Subject: [PATCH] Begin creating tests #16

---
 flight/avoidance/opensitl_multiple_drones.sh |  7 +++
 flight/avoidance/test.py                     | 57 ++++++++++++++++++++
 2 files changed, 64 insertions(+)
 create mode 100755 flight/avoidance/opensitl_multiple_drones.sh
 create mode 100644 flight/avoidance/test.py

diff --git a/flight/avoidance/opensitl_multiple_drones.sh b/flight/avoidance/opensitl_multiple_drones.sh
new file mode 100755
index 00000000..983d64eb
--- /dev/null
+++ b/flight/avoidance/opensitl_multiple_drones.sh
@@ -0,0 +1,7 @@
+cd ../../../PX4-Autopilot
+export PX4_HOME_LAT=37.9490953
+export PX4_HOME_LON=-91.7848293
+#export PX4_SIM_SPEED_FACTOR=1.5
+make px4_sitl_default
+gnome-terminal -- /bin/sh -c "./Tools/simulation/sitl_multiple_run.sh 2; ./Tools/simulation/jmavsim/jmavsim_run.sh -l"
+gnome-terminal -- ./Tools/simulation/jmavsim/jmavsim_run.sh -p 4561 -l
diff --git a/flight/avoidance/test.py b/flight/avoidance/test.py
new file mode 100644
index 00000000..5df240d2
--- /dev/null
+++ b/flight/avoidance/test.py
@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+
+import asyncio
+from mavsdk import System
+
+
+async def run():
+
+    drone = System()
+    print("Will connect")
+    await drone.connect(system_address="udp://:14540")
+    print("Connected")
+
+    status_text_task = asyncio.ensure_future(print_status_text(drone))
+
+    print("Waiting for drone to connect...")
+    async for state in drone.core.connection_state():
+        if state.is_connected:
+            print(f"-- Connected to drone!")
+            break
+
+    print("Waiting for drone to have a global position estimate...")
+    async for health in drone.telemetry.health():
+        if health.is_global_position_ok and health.is_home_position_ok:
+            print("-- Global position estimate OK")
+            break
+
+    print("-- Arming")
+    await drone.action.arm()
+
+    print("-- Taking off")
+    await drone.action.takeoff()
+
+    await asyncio.sleep(10)
+
+    print("-- Landing")
+    await drone.action.land()
+
+    status_text_task.cancel()
+
+
+
+async def print_status_text(drone):
+    try:
+        async for status_text in drone.telemetry.status_text():
+            print(f"Status: {status_text.type}: {status_text.text}")
+    except asyncio.CancelledError:
+        return
+
+
+if __name__ == "__main__":
+    try:
+        loop = asyncio.get_running_loop()
+    except RuntimeError:
+        loop = asyncio.new_event_loop()
+    asyncio.set_event_loop(loop)
+    asyncio.run(run())