• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/APM_Compass/APM_Compass.h

Go to the documentation of this file.
00001 #ifndef APM_Compass_h
00002 #define APM_Compass_h
00003 
00004 #include "../AP_Math/AP_Math.h"
00005 
00006 // Rotation matrices
00007 #define ROTATION_NONE                           Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
00008 #define ROTATION_YAW_45                         Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
00009 #define ROTATION_YAW_90                         Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
00010 #define ROTATION_YAW_135                        Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
00011 #define ROTATION_YAW_180                        Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
00012 #define ROTATION_YAW_225                        Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
00013 #define ROTATION_YAW_270                        Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
00014 #define ROTATION_YAW_315                        Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
00015 #define ROTATION_ROLL_180                       Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
00016 #define ROTATION_ROLL_180_YAW_45        Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1)
00017 #define ROTATION_ROLL_180_YAW_90        Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
00018 #define ROTATION_ROLL_180_YAW_135       Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1)
00019 #define ROTATION_PITCH_180                      Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
00020 #define ROTATION_ROLL_180_YAW_225       Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)
00021 #define ROTATION_ROLL_180_YAW_270       Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
00022 #define ROTATION_ROLL_180_YAW_315       Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
00023 
00024 // orientations for DIYDrones magnetometer
00025 #define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE
00026 #define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45
00027 #define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90
00028 #define APM_COMPASS_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135
00029 #define APM_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180
00030 #define APM_COMPASS_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225
00031 #define APM_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270
00032 #define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315
00033 #define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180
00034 #define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45
00035 #define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90
00036 #define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135
00037 #define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180
00038 #define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225
00039 #define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270
00040 #define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315
00041 
00042 // orientations for Sparkfun magnetometer
00043 #define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270
00044 #define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_315
00045 #define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE
00046 #define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_45
00047 #define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90
00048 #define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_135
00049 #define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180
00050 #define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_225
00051 #define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_90
00052 #define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_135
00053 #define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_RIGHT ROTATION_PITCH_180
00054 #define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_225
00055 #define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_270
00056 #define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_315
00057 #define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180
00058 #define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45
00059 
00060 class APM_Compass_Class
00061 {
00062   private:
00063         int orientation;
00064         Matrix3f orientationMatrix;
00065         float calibration[3];
00066         int offset[3];
00067         float declination;
00068   public:
00069         int Mag_X;
00070         int Mag_Y;
00071         int Mag_Z;
00072         float Heading;
00073         float Heading_X;
00074         float Heading_Y;
00075         unsigned long lastUpdate;       
00076         
00077         APM_Compass_Class();  // Constructor
00078         bool Init(int initialiseWireLib = 1);
00079         void Read();
00080         void Calculate(float roll, float pitch);
00081         void SetOrientation(const Matrix3f &rotationMatrix);
00082         void SetOffsets(int x, int y, int z);
00083         void SetDeclination(float radians);
00084 };
00085 
00086 class APM_Compass_HIL_Class
00087 {
00088   private:
00089         int orientation;
00090         Matrix3f orientationMatrix;
00091         float calibration[3];
00092         int offset[3];
00093         float declination;
00094   public:
00095         int Mag_X;
00096         int Mag_Y;
00097         int Mag_Z;
00098         float Heading;
00099         float Heading_X;
00100         float Heading_Y;
00101         unsigned long lastUpdate;       
00102         
00103         APM_Compass_HIL_Class();  // Constructor
00104         bool Init(int initialiseWireLib = 1);
00105         void Read();
00106         void Calculate(float roll, float pitch);
00107         void SetOrientation(const Matrix3f &rotationMatrix);
00108         void SetOffsets(int x, int y, int z);
00109         void SetDeclination(float radians);
00110     void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
00111 };
00112 
00113 #endif

Generated for ArduPilot Libraries by doxygen