Browse Source

AP_Proximity: Cycle through all drivers to check for upward distance

master
rishabsingh3003 2 years ago committed by Andrew Tridgell
parent
commit
3a347374c8
  1. 9
      libraries/AP_Proximity/AP_Proximity.cpp

9
libraries/AP_Proximity/AP_Proximity.cpp

@ -392,7 +392,14 @@ bool AP_Proximity::get_upward_distance(uint8_t instance, float &distance) const
bool AP_Proximity::get_upward_distance(float &distance) const bool AP_Proximity::get_upward_distance(float &distance) const
{ {
return get_upward_distance(primary_instance, distance); // get upward distance from backend
for (uint8_t i=0; i<num_instances; i++) {
// return first good upward distance
if (get_upward_distance(primary_instance, distance)) {
return true;
}
}
return false;
} }
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED

Loading…
Cancel
Save