From 4f136512f079a7b92ddd20b47e95cfa8afc15253 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sun, 10 Mar 2019 10:39:10 +1100
Subject: [PATCH] Tools: autotest: fix balancebot RTL test

---
 Tools/autotest/apmrover2.py  | 12 +++++++----
 Tools/autotest/balancebot.py | 40 ++++++++++++------------------------
 2 files changed, 21 insertions(+), 31 deletions(-)

diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py
index 97a5032ea4..5970a967ef 100644
--- a/Tools/autotest/apmrover2.py
+++ b/Tools/autotest/apmrover2.py
@@ -387,6 +387,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             "Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" %
             (distance_with_brakes, distance_without_brakes, delta))
 
+    def drive_rtl_mission_max_distance_from_home(self):
+        '''maximum distance allowed from home at end'''
+        return 6.5
+
     def drive_rtl_mission(self):
         self.wait_ready_to_arm()
         self.arm_vehicle()
@@ -408,8 +412,8 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         wp_dist_min = 5
         if m.wp_dist < wp_dist_min:
             raise PreconditionFailedException(
-                "Did not start at least %u metres from destination" %
-                (wp_dist_min))
+                "Did not start at least %f metres from destination (is=%f)" %
+                (wp_dist_min, m.wp_dist))
 
         self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" %
                       (m.wp_dist, wp_dist_min,))
@@ -420,7 +424,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
                 raise NotAchievedException("Did not get home")
             self.progress("Distance home: %f (mode=%s)" %
                           (self.distance_to_home(), self.mav.flightmode))
-            if self.mode_is('HOLD'):
+            if self.mode_is('HOLD') or self.mode_is('LOITER'): # loiter for balancebot
                 break
 
         # the EKF doesn't pull us down to 0 speed:
@@ -430,7 +434,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         # up ~6m past the home point.
         home_distance = self.distance_to_home()
         home_distance_min = 5.5
-        home_distance_max = 6.5
+        home_distance_max = self.drive_rtl_mission_max_distance_from_home()
         if home_distance > home_distance_max:
             raise NotAchievedException(
                 "Did not stop near home (%f metres distant (%f > want > %f))" %
diff --git a/Tools/autotest/balancebot.py b/Tools/autotest/balancebot.py
index e22e1e37cc..599e2ca4c6 100644
--- a/Tools/autotest/balancebot.py
+++ b/Tools/autotest/balancebot.py
@@ -4,42 +4,17 @@
 from __future__ import print_function
 
 import os
-import pexpect
 
 from apmrover2 import AutoTestRover
 from common import AutoTest
 
-from pymavlink import mavutil
-
 # get location of scripts
 testdir = os.path.dirname(os.path.realpath(__file__))
 
-# HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
-HOME = mavutil.location(40.071374969556928,
-                        -105.22978898137808,
-                        1583.702759,
-                        246)
-
+def log_name(self):
+    return "BalanceBot"
 
 class AutoTestBalanceBot(AutoTestRover):
-    def __init__(self,
-                 binary,
-                 valgrind=False,
-                 gdb=False,
-                 speedup=10,
-                 frame=None,
-                 params=None,
-                 gdbserver=False,
-                 **kwargs):
-        super(AutoTestBalanceBot, self).__init__(binary,
-                                                 valgrind,
-                                                 gdb,
-                                                 speedup,
-                                                 frame,
-                                                 params,
-                                                 gdbserver,
-                                                 **kwargs)
-        self.log_name = "BalanceBot"
 
     def vehicleinfo_key(self):
         return "APMrover2"
@@ -61,6 +36,17 @@ class AutoTestBalanceBot(AutoTestRover):
     def is_balancebot(self):
         return True
 
+    def drive_rtl_mission_max_distance_from_home(self):
+        '''maximum distance allowed from home at end'''
+        '''balancebot tends to wander backwards, away from the target'''
+        return 8
+
+    def drive_rtl_mission(self):
+        # if we Hold then the balancebot continues to wander
+        # indefinitely at ~1m/s
+        self.set_parameter("MIS_DONE_BEHAVE", 1)
+        super(AutoTestBalanceBot, self).drive_rtl_mission()
+
     def tests(self):
         '''return list of all tests'''