diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 8e636ed2b0..b336eb2014 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -453,6 +453,24 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Description: When enabled this will automatically check the orientation of compasses on successful completion of compass calibration. If set to 2 then external compasses will have their orientation automatically corrected. // @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix AP_GROUPINFO("AUTO_ROT", 35, Compass, _rotate_auto, HAL_COMPASS_AUTO_ROT_DEFAULT), + + // @Param: EXP_DID + // @DisplayName: Compass device id expected + // @Description: The expected value of COMPASS_DEV_ID, used by arming checks. Setting this to -1 means "don't care." + // @User: Advanced + AP_GROUPINFO("EXP_DID", 36, Compass, _state[0].expected_dev_id, -1), + + // @Param: EXP_DID2 + // @DisplayName: Compass2 device id expected + // @Description: The expected value of COMPASS_DEV_ID2, used by arming checks. Setting this to -1 means "don't care." + // @User: Advanced + AP_GROUPINFO("EXP_DID2", 37, Compass, _state[1].expected_dev_id, -1), + + // @Param: EXP_DID3 + // @DisplayName: Compass3 device id expected + // @Description: The expected value of COMPASS_DEV_ID3, used by arming checks. Setting this to -1 means "don't care." + // @User: Advanced + AP_GROUPINFO("EXP_DID3", 38, Compass, _state[2].expected_dev_id, -1), AP_GROUPEND }; @@ -1234,6 +1252,11 @@ bool Compass::configured(uint8_t i) return false; } + // if expected_dev_id is configured and the detected dev_id is different, return false + if (_state[i].expected_dev_id != -1 && _state[i].expected_dev_id != _state[i].dev_id) { + return false; + } + // if we got here then it must be configured return true; } diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index e8944a6f4f..7c1b12512f 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -431,6 +431,7 @@ private: // saved to eeprom when offsets are saved allowing ram & // eeprom values to be compared as consistency check AP_Int32 dev_id; + AP_Int32 expected_dev_id; int32_t detected_dev_id; AP_Int8 use_for_yaw;