From 45c393a25bbe1d50b8f81d13d1a142e692c6701a Mon Sep 17 00:00:00 2001
From: rishabsingh3003 <rishabh.singh100358@gmail.com>
Date: Mon, 4 Mar 2024 13:54:40 -0500
Subject: [PATCH 1/2] AP_Proximity: Improve get_status handling

---
 libraries/AP_Proximity/AP_Proximity.cpp | 20 ++++++++++++++------
 libraries/AP_Proximity/AP_Proximity.h   |  2 ++
 2 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index 82dc97e7919d0..753b7eb0bc030 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -308,17 +308,19 @@ AP_Proximity::Status AP_Proximity::get_instance_status(uint8_t instance) const
     return state[instance].status;
 }
 
+// Returns status of first good sensor. If no good sensor found, returns status of last instance sensor
 AP_Proximity::Status AP_Proximity::get_status() const
 {
+    Status sensors_status = Status::NotConnected;
     for (uint8_t i=0; i<num_instances; i++) {
-        const Status sensors_status = get_instance_status(i);
-        if (sensors_status != Status::Good) {
-            // return first bad status
+        sensors_status = get_instance_status(i);
+        if (sensors_status == Status::Good) {
+            // return first good status
             return sensors_status;
         }
     }
-    // All valid sensors seem to be working
-    return Status::Good;
+    // no good sensor found
+    return sensors_status;
 }
 
 // return proximity backend for Lua scripting
@@ -459,7 +461,13 @@ bool AP_Proximity::sensor_enabled() const
 
 bool AP_Proximity::sensor_failed() const
 {
-    return get_status() != Status::Good;
+    for (uint8_t i=0; i<num_instances; i++) {
+        if (get_instance_status(i) != Status::Good) {
+            // return first bad status
+            return true;
+        }
+    }
+    return false;
 }
 
 // get distance in meters upwards, returns true on success
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index 92d6f123e0793..61a52e02702ec 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -115,6 +115,8 @@ class AP_Proximity
 
     // return sensor health
     Status get_instance_status(uint8_t instance) const;
+
+    // Returns status of first good sensor. If no good sensor found, returns status of last instance sensor 
     Status get_status() const;
 
     // prearm checks

From e65c4c38d54e2f4a549d90c723e8b5d284b3e203 Mon Sep 17 00:00:00 2001
From: rishabsingh3003 <rishabh.singh100358@gmail.com>
Date: Mon, 4 Mar 2024 17:05:01 -0500
Subject: [PATCH 2/2] AC_Avoid: Remove sensor status checks

---
 libraries/AC_Avoidance/AC_Avoid.cpp | 11 -----------
 1 file changed, 11 deletions(-)

diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp
index 9745d4abb62b5..75f0aeb0fc77e 100644
--- a/libraries/AC_Avoidance/AC_Avoid.cpp
+++ b/libraries/AC_Avoidance/AC_Avoid.cpp
@@ -1139,10 +1139,6 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
     }
 
     AP_Proximity &_proximity = *proximity;
-    // check for status of the sensor
-    if (_proximity.get_status() != AP_Proximity::Status::Good) {
-        return;
-    }
     // get total number of obstacles
     const uint8_t obstacle_num = _proximity.get_obstacle_count();
     if (obstacle_num == 0) {
@@ -1434,14 +1430,7 @@ void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_ne
         return;
     }
     AP_Proximity &_proximity = *proximity;
-
-    // exit immediately if proximity sensor is not present
-    if (_proximity.get_status() != AP_Proximity::Status::Good) {
-        return;
-    }
-
     const uint8_t obj_count = _proximity.get_object_count();
-
     // if no objects return
     if (obj_count == 0) {
         return;