diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 6050790c8f..a32f6b040b 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -4,6 +4,7 @@ #endif #include #include +#include #include "AP_Compass_SITL.h" #include "AP_Compass_AK8963.h" @@ -476,7 +477,14 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @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), - + + // @Param: ENABLE + // @DisplayName: Enable Compass + // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1. + // @User: Standard + // @Values: 0:Disabled,1:Enabled + AP_GROUPINFO("ENABLE", 39, Compass, _enabled, 1), + AP_GROUPEND }; @@ -500,6 +508,10 @@ Compass::Compass(void) // void Compass::init() { + if (!AP::compass().enabled()) { + return; + } + if (_compass_count == 0) { // detect available backends. Only called once _detect_backends(); @@ -517,6 +529,17 @@ void Compass::init() for (uint8_t i=_compass_count; iprintf("Compass initialisation failed\n"); + AP::logger().Write_Error(LogErrorSubsystem::COMPASS, LogErrorCode::FAILED_TO_INITIALISE); + return; + } + + AP::ahrs().set_compass(this); } // Register a new compass instance diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 2781e5842c..2bf49c4858 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -76,6 +76,8 @@ public: /// bool read(); + bool enabled() const { return _enabled; } + /// Calculate the tilt-compensated heading_ variables. /// /// @param dcm_matrix The current orientation rotation matrix @@ -388,6 +390,9 @@ private: AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND]; uint8_t _backend_count; + // whether to enable the compass drivers at all + AP_Int8 _enabled; + // number of registered compasses. uint8_t _compass_count;