From b9d96ad14802c0aa1eaac24abc6544bda774ff0c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 3 Mar 2022 14:25:54 +1100
Subject: [PATCH] Tools: update runplanetest.py

added prearm check and cope with python3
---
 Tools/scripts/runplanetest.py | 39 +++++++++++++++++++++++++++--------
 1 file changed, 30 insertions(+), 9 deletions(-)

diff --git a/Tools/scripts/runplanetest.py b/Tools/scripts/runplanetest.py
index d94a55a243..4ceaa13bc1 100755
--- a/Tools/scripts/runplanetest.py
+++ b/Tools/scripts/runplanetest.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 import pexpect, time, sys
 from pymavlink import mavutil
@@ -25,6 +25,20 @@ def wait_mode(mav, modes, timeout=10):
     print("Failed to get mode from %s" % modes)
     sys.exit(1)
 
+def wait_prearm_ok(mav, timeout=30):
+    '''wait for pre-arm OK'''
+    start_time = time.time()
+    last_mode = None
+    while time.time() < start_time+timeout:
+        m = mav.recv_match(type='SYS_STATUS', blocking=True, timeout=2)
+        if m is None:
+            return
+        if m.onboard_control_sensors_health & mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK != 0:
+            print("Prearm OK")
+            return
+    print("Failed to get pre-arm OK")
+    sys.exit(1)
+    
 def wait_time(mav, simtime):
     '''wait for simulation time to pass'''
     imu = mav.recv_match(type='RAW_IMU', blocking=True)
@@ -36,17 +50,24 @@ def wait_time(mav, simtime):
             break
 
 cmd = '../Tools/autotest/sim_vehicle.py -D -f quadplane'
-mavproxy = pexpect.spawn(cmd, logfile=sys.stdout, timeout=30)
+mavproxy = pexpect.spawn(cmd, logfile=sys.stdout.buffer, timeout=30)
 mavproxy.expect("ArduPilot Ready")
 
 mav = mavutil.mavlink_connection('127.0.0.1:14550')
 
 mavproxy.send('speedup 40\n')
-mavproxy.expect('using GPS')
-mavproxy.expect('using GPS')
-mavproxy.expect('using GPS')
-mavproxy.expect('using GPS')
-mavproxy.send('auto\n')
+wait_prearm_ok(mav)
+mavproxy.send('mode guided\n')
+wait_mode(mav, ['GUIDED'])
 mavproxy.send('arm throttle\n')
-wait_mode(mav, ['AUTO'])
-mavproxy.expect("Mission: 5 WP")
+mavproxy.send('takeoff 40\n')
+wait_time(mav, 30)
+mavproxy.send('mode cruise\n')
+wait_mode(mav, ['CRUISE'])
+wait_time(mav, 10)
+mavproxy.send('mode qrtl\n')
+wait_mode(mav, ['QRTL'])
+mavproxy.send('module load console\n')
+mavproxy.send('module load map\n')
+mavproxy.logfile = None
+mavproxy.interact()