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
This commit is contained in:
Andrew Tridgell 2019-10-24 09:03:55 +11:00
parent b19ad689a6
commit df5e154144
1 changed files with 12 additions and 5 deletions

View File

@ -570,16 +570,23 @@ 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->orientation() != orientation) {
continue;
}
if (backend != nullptr &&
backend->orientation() == orientation &&
backend->status() == RangeFinder_Good) {
return backend;
}
}
// 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 nullptr;
}