Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -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;
}