diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index b3cb985d91..982e72bb28 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/libraries/AP_Compass/AP_Compass_HIL.h @@ -7,7 +7,7 @@ class AP_Compass_HIL : public Compass { public: - AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) { product_id = AP_COMPASS_TYPE_HIL; } + AP_Compass_HIL() : Compass() { product_id = AP_COMPASS_TYPE_HIL; } bool read(void); void setHIL(float Mag_X, float Mag_Y, float Mag_Z); }; diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 6a160230bd..12809d74a8 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -75,7 +75,7 @@ bool AP_Compass_HMC5843::write_register(uint8_t address, byte value) the 5883L has a different orientation to the 5843. This allows us to use a single MAG_ORIENTATION for both */ -static void rotate_for_5883L(AP_VarS *_orientation_matrix) +static void rotate_for_5883L(AP_Matrix3f *_orientation_matrix) { _orientation_matrix->set_and_save(_orientation_matrix->get() * Matrix3f(ROTATION_YAW_90)); } diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index 2549f38a8f..d5ef7e967d 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -56,7 +56,7 @@ class AP_Compass_HMC5843 : public Compass uint32_t _retry_time; // when unhealthy the millis() value to retry at public: - AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} + AP_Compass_HMC5843() : Compass() {} virtual bool init(void); virtual bool read(void); virtual void set_orientation(const Matrix3f &rotation_matrix); diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 4d5d69a9c4..848de698d0 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -1,15 +1,19 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Compass.h" +const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { + { AP_PARAM_MATRIX3F, "", VAROFFSET(Compass, _orientation_matrix) }, + { AP_PARAM_VECTOR3F, "", VAROFFSET(Compass, _offset) }, + { AP_PARAM_VECTOR3F, "DEC", VAROFFSET(Compass, _declination) }, + { AP_PARAM_NONE, "" } +}; + // Default constructor. // Note that the Vector/Matrix constructors already implicitly zero // their values. // -Compass::Compass(AP_Var::Key key) : - _group(key, PSTR("COMPASS_")), - _orientation_matrix (&_group, 0), - _offset (&_group, 1), - _declination (&_group, 2, 0.0, PSTR("DEC")), +Compass::Compass(void) : + _declination (0.0), _null_init_done(false), _null_enable(false), product_id(AP_COMPASS_TYPE_UNKNOWN) @@ -50,7 +54,7 @@ Compass::save_offsets() Vector3f & Compass::get_offsets() { - return _offset.get(); + return _offset; } void @@ -183,4 +187,4 @@ Compass::null_offsets_disable(void) { _null_init_done = false; _null_enable = false; -} \ No newline at end of file +} diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 67a533fb02..897068c490 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -47,7 +47,7 @@ public: /// /// @param key Storage key used for configuration data. /// - Compass(AP_Var::Key key); + Compass(); /// Initialize the compass device. /// @@ -132,10 +132,11 @@ public: virtual void set_declination(float radians); float get_declination(); + static const struct AP_Param::GroupInfo var_info[]; + protected: - AP_Var_group _group; ///< storage group holding the compass' calibration data - AP_VarS _orientation_matrix; - AP_VarS _offset; + AP_Matrix3f _orientation_matrix; + AP_Vector3f _offset; AP_Float _declination; bool _null_enable; ///< enabled flag for offset nulling