00001 #ifndef AP_Compass_HMC5843_H
00002 #define AP_Compass_HMC5843_H
00003
00004 #include "Compass.h"
00005 #include "../AP_Math/AP_Math.h"
00006
00007
00008 #define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
00009 #define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
00010 #define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
00011 #define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
00012 #define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
00013 #define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
00014 #define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
00015 #define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
00016 #define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
00017 #define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1)
00018 #define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
00019 #define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1)
00020 #define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
00021 #define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)
00022 #define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
00023 #define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
00024
00025
00026 #define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE
00027 #define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45
00028 #define AP_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90
00029 #define AP_COMPASS_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135
00030 #define AP_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180
00031 #define AP_COMPASS_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225
00032 #define AP_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270
00033 #define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315
00034 #define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180
00035 #define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45
00036 #define AP_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90
00037 #define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135
00038 #define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180
00039 #define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225
00040 #define AP_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270
00041 #define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315
00042
00043
00044 #define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270
00045 #define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_315
00046 #define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE
00047 #define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_45
00048 #define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90
00049 #define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_135
00050 #define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180
00051 #define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_225
00052 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_90
00053 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_135
00054 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_RIGHT ROTATION_PITCH_180
00055 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_225
00056 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_270
00057 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_315
00058 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180
00059 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45
00060
00061 class AP_Compass_HMC5843 : public Compass
00062 {
00063 private:
00064 int orientation;
00065 Matrix3f orientation_matrix;
00066 float calibration[3];
00067 int offset[3];
00068 float declination;
00069 public:
00070 AP_Compass_HMC5843();
00071 bool init(int initialiseWireLib = 1);
00072 void read();
00073 void calculate(float roll, float pitch);
00074 void set_orientation(const Matrix3f &rotation_matrix);
00075 void set_offsets(int x, int y, int z);
00076 void set_declination(float radians);
00077 };
00078 #endif