00001 #ifndef APM_Compass_h
00002 #define APM_Compass_h
00003
00004 #include "../AP_Math/AP_Math.h"
00005
00006
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
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
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();
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();
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