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

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

This commit is contained in:
bugobliterator 2020-06-05 15:17:54 +05:30 committed by Andrew Tridgell
parent a58415391f
commit a93b93b741
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
{
#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++) {
if (_priority_did_list[priority] == _state[i].expected_dev_id) {
return i;
@ -1569,7 +1572,8 @@ bool Compass::configured(uint8_t i)
StateIndex id = _get_state_id(Priority(i));
// 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;
}