diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 0df6cc0f3c..33b9ac304c 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -169,6 +169,15 @@ Compass::init() return true; } +void +Compass::set_offsets(uint8_t i, const Vector3f &offsets) +{ + // sanity check compass instance provided + if (i < COMPASS_MAX_INSTANCES) { + _offset[i].set(offsets); + } +} + void Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets) { diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 71b3fdb4b5..1a0af22927 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -87,6 +87,13 @@ public: /// float calculate_heading(const Matrix3f &dcm_matrix) const; + /// Sets offset x/y/z values. + /// + /// @param i compass instance + /// @param offsets Offsets to the raw mag_ values. + /// + void set_offsets(uint8_t i, const Vector3f &offsets); + /// Sets and saves the compass offset x/y/z values. /// /// @param i compass instance