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

AP_Compass: fix reordering compass devid by priority at boot

This commit is contained in:
Siddharth Purohit 2020-08-16 15:50:06 +05:30 committed by Randy Mackay
parent 6a30a0fb12
commit fd125fef1f
2 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Compass/AP_Compass.cpp
View File

@ -732,7 +732,9 @@ void Compass::init()
} }
#endif #endif
#if COMPASS_MAX_INSTANCES > 1
_reorder_compass_params(); _reorder_compass_params();
#endif
if (_compass_count == 0) { if (_compass_count == 0) {
// detect available backends. Only called once // detect available backends. Only called once
@ -801,6 +803,7 @@ Compass::Priority Compass::_update_priority_list(int32_t dev_id)
#endif #endif
#if COMPASS_MAX_INSTANCES > 1
// This method reorganises devid list to match // This method reorganises devid list to match
// priority list, only call before detection at boot // priority list, only call before detection at boot
void Compass::_reorder_compass_params() void Compass::_reorder_compass_params()
@ -808,7 +811,16 @@ void Compass::_reorder_compass_params()
mag_state swap_state; mag_state swap_state;
StateIndex curr_state_id; StateIndex curr_state_id;
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) { for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
curr_state_id = _get_state_id(i); if (_priority_did_list[i] == 0) {
continue;
}
curr_state_id = COMPASS_MAX_INSTANCES;
for (StateIndex j(0); j<COMPASS_MAX_INSTANCES; j++) {
if (_priority_did_list[i] == _state[j].dev_id) {
curr_state_id = j;
break;
}
}
if (curr_state_id != COMPASS_MAX_INSTANCES && uint8_t(curr_state_id) != uint8_t(i)) { if (curr_state_id != COMPASS_MAX_INSTANCES && uint8_t(curr_state_id) != uint8_t(i)) {
//let's swap //let's swap
swap_state.copy_from(_state[curr_state_id]); swap_state.copy_from(_state[curr_state_id]);
@ -817,6 +829,7 @@ void Compass::_reorder_compass_params()
} }
} }
} }
#endif
void Compass::mag_state::copy_from(const Compass::mag_state& state) void Compass::mag_state::copy_from(const Compass::mag_state& state)
{ {

2
libraries/AP_Compass/AP_Compass.h
View File

@ -522,7 +522,9 @@ private:
void _detect_runtime(void); void _detect_runtime(void);
// This method reorganises devid list to match // This method reorganises devid list to match
// priority list, only call before detection at boot // priority list, only call before detection at boot
#if COMPASS_MAX_INSTANCES > 1
void _reorder_compass_params(); void _reorder_compass_params();
#endif
// Update Priority List for Mags, by default, we just // Update Priority List for Mags, by default, we just
// load them as they come up the first time // load them as they come up the first time
Priority _update_priority_list(int32_t dev_id); Priority _update_priority_list(int32_t dev_id);