0,0 → 1,137 |
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
#ifndef Compass_h |
#define Compass_h |
|
#include <inttypes.h> |
#include "AP_Math.h" |
|
// compass product id |
#define AP_COMPASS_TYPE_UNKNOWN 0x00 |
#define AP_COMPASS_TYPE_HIL 0x01 |
#define AP_COMPASS_TYPE_HMC5843 0x02 |
#define AP_COMPASS_TYPE_HMC5883L 0x03 |
|
class Compass |
{ |
public: |
int16_t product_id; /// product id |
int16_t mag_x; ///< magnetic field strength along the X axis |
int16_t mag_y; ///< magnetic field strength along the Y axis |
int16_t mag_z; ///< magnetic field strength along the Z axis |
float heading; ///< compass heading in radians |
float heading_x; ///< compass vector X magnitude |
float heading_y; ///< compass vector Y magnitude |
uint32_t last_update; ///< micros() time of last update |
bool healthy; ///< true if last read OK |
|
/// Constructor |
/// |
Compass(); |
|
/// Initialize the compass device. |
/// |
/// @returns True if the compass was initialized OK, false if it was not |
/// found or is not functioning. |
/// |
virtual bool init(); |
|
/// Read the compass and update the mag_ variables. |
/// |
virtual bool read(void) /* = 0 */; |
|
/// Calculate the tilt-compensated heading_ variables. |
/// |
/// @param roll The current airframe roll angle. |
/// @param pitch The current airframe pitch angle. |
/// |
// virtual void calculate(float roll, float pitch); |
|
/// Calculate the tilt-compensated heading_ variables. |
/// |
/// @param dcm_matrix The current orientation rotation matrix |
/// |
virtual void calculate(const Matrix3f &dcm_matrix); |
|
/// Set the compass orientation matrix, used to correct for |
/// various compass mounting positions. |
/// |
/// @param rotation_matrix Rotation matrix to transform magnetometer readings |
/// to the body frame. |
/// |
//virtual void set_orientation(enum Rotation rotation); |
|
/// Sets the compass offset x/y/z values. |
/// |
/// @param offsets Offsets to the raw mag_ values. |
/// |
// virtual void set_offsets(const Vector3f &offsets); |
|
/// Saves the current compass offset x/y/z values. |
/// |
/// This should be invoked periodically to save the offset values maintained by |
/// ::null_offsets. |
/// |
// virtual void save_offsets(); |
|
/// Returns the current offset values |
/// |
/// @returns The current compass offsets. |
/// |
// virtual Vector3f &get_offsets(); |
|
/// Sets the initial location used to get declination |
/// |
/// @param latitude GPS Latitude. |
/// @param longitude GPS Longitude. |
/// |
// void set_initial_location(long latitude, long longitude); |
|
/// Program new offset values. |
/// |
/// @param x Offset to the raw mag_x value. |
/// @param y Offset to the raw mag_y value. |
/// @param z Offset to the raw mag_z value. |
/// |
// void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); } |
|
/// Perform automatic offset updates |
/// |
// void null_offsets(void); |
|
/// Enable/Start automatic offset updates |
/// |
void null_offsets_enable(void); |
|
|
/// Disable/Stop automatic offset updates |
/// |
void null_offsets_disable(void); |
|
/// return true if the compass should be used for yaw calculations |
bool use_for_yaw(void) { return healthy && _use_for_yaw; } |
|
/// Sets the local magnetic field declination. |
/// |
/// @param radians Local field declination. |
/// |
// virtual void set_declination(float radians); |
float get_declination(); |
|
// static const struct AP_Param::GroupInfo var_info[]; |
|
protected: |
float _declination; |
int8_t _learn; ///<enable calibration learning |
int8_t _use_for_yaw; ///<enable use for yaw calculation |
bool _null_enable; ///< enabled flag for offset nulling |
bool _null_init_done; ///< first-time-around flag used by offset nulling |
int8_t _auto_declination; ///<enable automatic declination code |
enum Rotation _orientation; |
|
Vector3f _offset; |
///< used by offset correction |
static const uint8_t _mag_history_size = 20; |
uint8_t _mag_history_index; |
Vector3i _mag_history[_mag_history_size]; |
}; |
#endif |