IMUCalc.h

00001 /*Seriously no one cares about copyright stuff
00002 
00003 But if you really want the license: If you make lots of money with my code, send me money, ty
00004 
00005 Now usefull stuff*/
00006 
00007 
00008 #ifndef IMUCALC_H
00009 #define IMUCALC_H
00010 
00011 /**
00012  * Includes
00013  */
00014 #include "mbed.h"
00015 #include "math.h"
00016 #include "GTMath.h"
00017 
00018 #ifndef M_PI
00019 #define M_PI           3.14159265358979323846
00020 #endif
00021 
00022 class IMUCalc {
00023 public:
00024     IMUCalc( void );
00025     
00026     /**
00027      * Calculates new headings
00028      *
00029      * @param accdat - pointer to float array with length 3 with acceleration data
00030      * @param gyrodat - pointer to float array with length 3 with gyroscope data
00031      * @param magdat - pointer to float array with length 3 with magnetic data
00032      * @param timestep - float with timestep in seconds
00033      */
00034     void runCalc(float *accdat, float *gyrodat, float *magdat, float timestep);
00035     
00036     /**
00037     * Returns the yaw
00038     *
00039     * @param return - float with yaw in rads
00040     */
00041     float getYaw( void );
00042     
00043     /**
00044     * Returns the pitch
00045     *
00046     * @param return - float with pitch in rads
00047     */
00048     float getPitch( void );
00049     
00050     /**
00051     * Returns the roll
00052     *
00053     * @param return - float with roll in rads
00054     */
00055     float getRoll( void );
00056     
00057     /**
00058     * Returns the calculated offset of the gyro
00059     *
00060     * @param return - Vector3 with gyro offsets
00061     */
00062     Vector3 getGyroOffset( void );
00063 
00064 
00065 
00066 //private:
00067     /**
00068      * Rotates magnetic vector to be in plane
00069      *
00070      * @param magdat - Vector3 with magnetic data
00071      * @param ground - Vector3 with data which direction the ground is
00072      *
00073      * @param return - Vector3 with new magnetic vector
00074      */
00075     Vector3 rotateMag(Vector3 magdat, Vector3 ground);
00076     
00077     /**
00078      * Calculates the angle between two vectors
00079      *
00080      * @param vectorA - first vector
00081      * @param vectorB - second vector
00082      * @param return - vector with which to rotate vectorA to get vectorB
00083      */
00084     static Vector3 angleBetween(Vector3 vectorA, Vector3 VectorB);
00085     
00086     /**
00087      * Rotate a vector around an axis
00088      *
00089      * @param vector - vector to rotate
00090      * @param axis - axis to rotate it around
00091      * @param angle - angle to rotate the vectors
00092      * @param return - resulting vector
00093      */
00094     static Vector3 rotateVector(Vector3 vector, Vector3 axis, float angle);
00095 
00096 
00097     Vector3 heading;
00098     Vector3 top;
00099     Vector3 gyroOffset;
00100     
00101     float absGain;
00102     bool initialRun;
00103 
00104 };
00105 
00106 #endif