From df5e154144a585561073ed2e1b6e1ac8ea81c5bb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 24 Oct 2019 09:03:55 +1100
Subject: [PATCH] AP_RangeFinder: fixed failover between rangefinders

this fixes the case where we have one rangefinder that can handle
short range and another that is good for long range but no good for
short range (quite common, eg radar and lidar)

If possible we want to use the first rangefinder that is in range for
the right orientation. If none are in range then use the first for the
orientation
---
 libraries/AP_RangeFinder/RangeFinder.cpp | 17 ++++++++++++-----
 1 file changed, 12 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 5ce6a9f8e7..a0a012b91c 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -570,15 +570,22 @@ bool RangeFinder::has_orientation(enum Rotation orientation) const
 // find first range finder instance with the specified orientation
 AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) const
 {
+    // first try for a rangefinder that is in range
     for (uint8_t i=0; i<num_instances; i++) {
         AP_RangeFinder_Backend *backend = get_backend(i);
-        if (backend == nullptr) {
-            continue;
+        if (backend != nullptr &&
+            backend->orientation() == orientation &&
+            backend->status() == RangeFinder_Good) {
+            return backend;
         }
-        if (backend->orientation() != orientation) {
-            continue;
+    }
+    // if none in range then return first with correct orientation
+    for (uint8_t i=0; i<num_instances; i++) {
+        AP_RangeFinder_Backend *backend = get_backend(i);
+        if (backend != nullptr &&
+            backend->orientation() == orientation) {
+            return backend;
         }
-        return backend;
     }
     return nullptr;
 }