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

AP_Compass: return id of max empty compass state when prio id is 0

Browse Source
This commit is contained in:
bugobliterator 2020-06-05 15:17:54 +05:30 committed by Randy Mackay
parent e0e84c34ee
commit 46b078a43f
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Compass/AP_Compass.cpp
View File

@ -896,6 +896,9 @@ bool Compass::register_compass(int32_t dev_id, uint8_t& instance)
Compass::StateIndex Compass::_get_state_id(Compass::Priority priority) const Compass::StateIndex Compass::_get_state_id(Compass::Priority priority) const
{ {
#if COMPASS_MAX_INSTANCES > 1 #if COMPASS_MAX_INSTANCES > 1
if (_priority_did_list[priority] == 0) {
return StateIndex(COMPASS_MAX_INSTANCES);
}
for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) { for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_priority_did_list[priority] == _state[i].expected_dev_id) { if (_priority_did_list[priority] == _state[i].expected_dev_id) {
return i; return i;
@ -1569,7 +1572,8 @@ bool Compass::configured(uint8_t i)
StateIndex id = _get_state_id(Priority(i)); StateIndex id = _get_state_id(Priority(i));
// exit immediately if dev_id hasn't been detected // exit immediately if dev_id hasn't been detected
if (_state[id].detected_dev_id == 0) { if (_state[id].detected_dev_id == 0 ||
id == COMPASS_MAX_INSTANCES) {
return false; return false;
} }