Browse Source

AP_Compass: added set_offsets() interface

this will be used by Replay to prevent the need for saving parameters
master
Andrew Tridgell 10 years ago
parent
commit
b437977547
  1. 9
      libraries/AP_Compass/Compass.cpp
  2. 7
      libraries/AP_Compass/Compass.h

9
libraries/AP_Compass/Compass.cpp

@ -289,6 +289,15 @@ Compass::init() @@ -289,6 +289,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)
{

7
libraries/AP_Compass/Compass.h

@ -87,6 +87,13 @@ public: @@ -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

Loading…
Cancel
Save