Subversion Repositories FlightCtrl

/// -*- 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