aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RangeFinder: fixed failover between rangefinders

Browse Source 
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 committed by Randy Mackay
parent f7cbdb4520
commit 8e0124f383
1 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -544,15 +544,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;
}