|
|
|
@ -1440,12 +1440,14 @@ bool Compass::is_replacement_mag(uint32_t devid) {
@@ -1440,12 +1440,14 @@ bool Compass::is_replacement_mag(uint32_t devid) {
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#if COMPASS_MAX_UNREG_DEV > 0 |
|
|
|
|
// Check that its not an unused additional mag
|
|
|
|
|
for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) { |
|
|
|
|
if (_previously_unreg_mag[i] == devid) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
// Check that its not previously setup mag
|
|
|
|
|
for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) { |
|
|
|
@ -1465,6 +1467,7 @@ void Compass::remove_unreg_dev_id(uint32_t devid)
@@ -1465,6 +1467,7 @@ void Compass::remove_unreg_dev_id(uint32_t devid)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#if COMPASS_MAX_UNREG_DEV > 0 |
|
|
|
|
for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) { |
|
|
|
|
if ((uint32_t)extra_dev_id[i] == devid) { |
|
|
|
|
extra_dev_id[i].set_and_save(0); |
|
|
|
@ -1472,6 +1475,7 @@ void Compass::remove_unreg_dev_id(uint32_t devid)
@@ -1472,6 +1475,7 @@ void Compass::remove_unreg_dev_id(uint32_t devid)
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Compass::_reset_compass_id() |
|
|
|
@ -1486,7 +1490,7 @@ void Compass::_reset_compass_id()
@@ -1486,7 +1490,7 @@ void Compass::_reset_compass_id()
|
|
|
|
|
} |
|
|
|
|
if (!_get_state(i).registered) { |
|
|
|
|
_priority_did_stored_list[i].set_and_save(0); |
|
|
|
|
gcs().send_text(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu removed", uint8_t(i), (unsigned long)_priority_did_list[i]); |
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu removed", uint8_t(i), (unsigned long)_priority_did_list[i]); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|