Tools: autotest: add test for ekf navigation speed limits

Tools/autotest/arducopter.py

@@ -142,14 +142,17 @@ class AutoTestCopter(AutoTest):
         self.progress("Ran command")
         self.wait_for_alt(alt_min)
 
-    def takeoff(self, alt_min=30, takeoff_throttle=1700):
+    def takeoff(self,
+                alt_min=30,
+                takeoff_throttle=1700,
+                require_absolute=True):
         """Takeoff get to 30m altitude."""
         self.progress("TAKEOFF")
         self.mavproxy.send('switch 6\n')  # stabilize mode
         self.wait_mode('STABILIZE')
         if not self.armed():
             self.progress("Waiting reading for arm")
-            self.wait_ready_to_arm()
+            self.wait_ready_to_arm(require_absolute=require_absolute)
             self.set_rc(3, 1000)
             self.arm_vehicle()
         self.set_rc(3, takeoff_throttle)
@@ -951,6 +954,57 @@ class AutoTestCopter(AutoTest):
 
         self.progress("CIRCLE OK for %u seconds" % holdtime)
 
+    # fly_optical_flow_limits - test EKF navigation limiting
+    def fly_optical_flow_limits(self):
+        ex = None
+        self.context_push()
+        try:
+            self.set_parameter("SIM_FLOW_ENABLE", 1)
+            self.set_parameter("FLOW_ENABLE", 1)
+
+            self.set_parameter("RNGFND_TYPE", 1)
+            self.set_parameter("RNGFND_MIN_CM", 0)
+            self.set_parameter("RNGFND_MAX_CM", 4000)
+            self.set_parameter("RNGFND_PIN", 0)
+            self.set_parameter("RNGFND_SCALING", 12.12, epsilon=0.01)
+
+            self.set_parameter("SIM_GPS_DISABLE", 1)
+
+            self.reboot_sitl()
+
+            self.takeoff(alt_min=2, require_absolute=False)
+
+            self.mavproxy.send('mode loiter\n')
+            self.wait_mode('LOITER')
+
+            # speed should be limited to <10m/s
+            self.set_rc(2, 1000)
+
+            tstart = self.get_sim_time()
+            timeout = 60
+            while self.get_sim_time_cached() - tstart < timeout:
+                m = self.mav.recv_match(type='VFR_HUD', blocking=True)
+                spd = m.groundspeed
+                self.progress("%0.1f: Low Speed: %f" %
+                              (self.get_sim_time_cached() - tstart, spd))
+                if spd > 8:
+                    self.progress("Speed should be limited by EKF optical flow limits")
+                    raise NotAchievedException()
+
+            self.progress("Moving higher")
+            self.change_alt(60)
+
+            self.wait_groundspeed(10, 100, timeout=60)
+        except Exception as e:
+            ex = e
+
+        self.set_rc(2, 1500)
+        self.context_pop()
+        self.reboot_sitl()
+
+        if ex is not None:
+            raise ex
+
     # fly_autotune - autotune the virtual vehicle
     def fly_autotune(self):
 
@@ -1893,6 +1947,10 @@ class AutoTestCopter(AutoTest):
             # Circle mode
             self.run_test("Fly CIRCLE mode", self.fly_circle)
 
+            # Optical flow limits test
+            self.run_test("Fly Optical Flow limits",
+                          self.fly_optical_flow_limits)
+
             # RTL
             self.run_test("RTL after CIRCLE mode", self.fly_RTL)
 

Tools/autotest/common.py

@@ -640,13 +640,14 @@ class AutoTest(ABC):
             return True
         return False
 
-    def set_parameter(self, name, value, add_to_context=True):
+    def set_parameter(self, name, value, add_to_context=True, epsilon=0.00001):
         """Set parameters from vehicle."""
         old_value = self.get_parameter(name, retry=2)
         for i in range(1, 10):
             self.mavproxy.send("param set %s %s\n" % (name, str(value)))
             returned_value = self.get_parameter(name)
-            if returned_value == float(value):
+            delta = float(value) - returned_value
+            if abs(delta) < epsilon:
                 # yes, exactly equal.
                 if add_to_context:
                     self.context_get().parameters.append((name, old_value))
@@ -1158,12 +1159,13 @@ class AutoTest(ABC):
         # self.mav.flightmode, mode))
         self.progress("Got mode %s" % mode)
 
-    def wait_ready_to_arm(self, timeout=None):
+    def wait_ready_to_arm(self, timeout=None, require_absolute=True):
         # wait for EKF checks to pass
         self.progress("Waiting reading for arm")
-        return self.wait_ekf_happy(timeout=timeout)
+        return self.wait_ekf_happy(timeout=timeout,
+                                   require_absolute=require_absolute)
 
-    def wait_ekf_happy(self, timeout=30):
+    def wait_ekf_happy(self, timeout=30, require_absolute=True):
         """Wait for EKF to be happy"""
 
         """ if using SITL estimates directly """
@@ -1176,14 +1178,16 @@ class AutoTest(ABC):
                           mavutil.mavlink.ESTIMATOR_VELOCITY_HORIZ |
                           mavutil.mavlink.ESTIMATOR_VELOCITY_VERT |
                           mavutil.mavlink.ESTIMATOR_POS_HORIZ_REL |
-                          mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS |
+                          mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_REL)
-                          mavutil.mavlink.ESTIMATOR_POS_VERT_ABS |
-                          mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_REL |
-                          mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_ABS)
         # none of these bits must be set for arming to happen:
         error_bits = (mavutil.mavlink.ESTIMATOR_CONST_POS_MODE |
-                      mavutil.mavlink.ESTIMATOR_GPS_GLITCH |
                       mavutil.mavlink.ESTIMATOR_ACCEL_ERROR)
+        if require_absolute:
+            required_value |= (mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS |
+                               mavutil.mavlink.ESTIMATOR_POS_VERT_ABS |
+                               mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_ABS)
+            error_bits |= mavutil.mavlink.ESTIMATOR_GPS_GLITCH
+
         self.progress("Waiting for EKF value %u" % required_value)
         last_err_print_time = 0
         last_print_time = 0