Basic functionality

Revision 1:51c902d63dda, committed 2014-01-22

Comitter:

Sissors

Date:

Wed Jan 22 21:19:18 2014 +0000

Parent:

0:4dc7e56179ff

Commit message:

v0.1

Changed in this revision

--- a/GTMath.lib	Wed Jul 11 12:26:44 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/BlazeX/libraries/GTMath/mayy5n
\ No newline at end of file

--- a/IMUCalc.cpp	Wed Jul 11 12:26:44 2012 +0000
+++ b/IMUCalc.cpp	Wed Jan 22 21:19:18 2014 +0000
@@ -1,22 +1,252 @@
 #include "IMUCalc.h"
 
-Vector3 IMUCalc::angleBetween(Vector3 vectorA, Vector3 vectorB){
-    float angle; 
-	
-	angle = vectorA.Angle(vectorB); 
-	// if no noticable rotation is available return zero rotation
-	// this way we avoid Cross product artifacts 
-	if( abs(angle) < 0.0001 ) return Vector3( 0, 0, 0); 
-	// in this case there are 2 lines on the same axis 
-	if(abs(angle-M_PI) < 0.001) {
-	    //They are in opposite directions, rotate one by 90 degrees, that picks one of the infinite amount of rotation angles you get
-	    float temp = vectorB.z;
-  	    vectorB.z=vectorB.y;
-  	    vectorB.y=vectorB.x;
-  	    vectorB.x=temp;
-	}
- 	Vector3 axis = (vectorA.CrossP(vectorB));
-	axis=axis.Normalize();
-	axis *= (angle);
-	return axis; 
-}
\ No newline at end of file
+IMUCalc::IMUCalc(void)
+{
+    heading.x=1;
+    heading.y=0;
+    heading.z=0;
+
+    top.x=0;
+    top.y=0;
+    top.z=1;
+
+    gyroOffset.x=0;
+    gyroOffset.y=0;
+    gyroOffset.z=0;
+
+    absGain=0.01;
+    initialRun=true;
+
+
+}
+
+
+void IMUCalc::runCalc(float *accdat, float *gyrodat, float *magdat, float timestep)
+{
+
+
+    //Variables
+    Vector3 accdata;
+    Vector3 gyrodata;
+    Vector3 magdata;
+
+    Vector3 heading_abs;
+    Vector3 top_abs;
+
+    //Change data to vector3
+    for (int i = 0; i<3; i++) {
+        accdata.af[i]=accdat[i];
+        gyrodata.af[i]=gyrodat[i];
+        magdata.af[i]=magdat[i];
+    }
+
+    gyrodata = gyrodata-gyroOffset; 
+
+    heading = rotateVector(heading, gyrodata, -gyrodata.Length()*timestep);
+    top = rotateVector(top, gyrodata, -gyrodata.Length()*timestep);
+
+
+
+    
+
+    //Rotate the magnetic data to be in plain with the earth
+    heading_abs  = -1 * rotateMag(magdata, accdata);
+    top_abs = -1 * accdata;
+
+    heading_abs = heading_abs.Normalize();
+    top_abs = top_abs.Normalize();
+
+    //Calculate offset
+    Vector3 currentGyroOffset, weightTop, weightHeading, tempVector;
+
+
+
+    //make tempvector in X direction, do crossproduct, calculate length of result
+    tempVector.x = 1;
+    tempVector.y=0;
+    tempVector.z=0;
+    weightTop.x = top.CrossP(tempVector).Length();
+    weightHeading.x = heading.CrossP(tempVector).Length();
+
+    tempVector.x = 0;
+    tempVector.y=1;
+    tempVector.z=0;
+    weightTop.y = top.CrossP(tempVector).Length();
+    weightHeading.y = heading.CrossP(tempVector).Length();
+
+    tempVector.x = 0;
+    tempVector.y=0;
+    tempVector.z=1;
+    weightTop.z = top.CrossP(tempVector).Length();
+    weightHeading.z = heading.CrossP(tempVector).Length();
+
+
+    //Use weightfactors, then divide by their sum
+    currentGyroOffset = weightTop * angleBetween(top_abs, top) + weightHeading * angleBetween(heading_abs, heading);
+    currentGyroOffset = currentGyroOffset / (weightTop + weightHeading);
+
+
+    if (currentGyroOffset.x > timestep * 0.1)
+        currentGyroOffset.x = timestep * 0.1;
+    if (currentGyroOffset.x < -timestep * 0.1)
+        currentGyroOffset.x = -timestep * 0.1;
+
+    if (currentGyroOffset.y > timestep * 0.1)
+        currentGyroOffset.y = timestep * 0.1;
+    if (currentGyroOffset.y < -timestep * 0.1)
+        currentGyroOffset.y = -timestep * 0.1;
+
+    if (currentGyroOffset.z > timestep * 0.1)
+        currentGyroOffset.z = timestep * 0.1;
+    if (currentGyroOffset.z < -timestep * 0.1)
+        currentGyroOffset.z = -timestep * 0.1;
+
+    gyroOffset -= 0.01 * currentGyroOffset;
+
+
+    //Take average value of heading/heading_abs with different gains to get current estimate
+    if (initialRun) {
+        heading = heading_abs;
+        top = top_abs;
+        gyroOffset *= 0;
+        initialRun=false;
+    } else {
+        heading = heading*(1-absGain) + heading_abs*absGain;
+        top = top * (1-absGain) + top_abs * absGain;
+    }
+}
+
+//Calculates the yaw
+float IMUCalc::getYaw( void )
+{
+    //First normalize yaw vector, then calculate the heading
+    Vector2 yawVector(heading.x, heading.y);
+
+    if (yawVector.Length()>0) {
+        yawVector = yawVector.Normalize();
+
+        //check Quadrant
+        if (yawVector.y<0) {
+            if (yawVector.x < 0)
+                return -M_PI - asin(yawVector.y);
+            else
+                return asin(yawVector.y);
+        } else {
+            if (yawVector.x < 0)
+                return M_PI - asin(yawVector.y);
+            else
+                return asin(yawVector.y);
+        }
+    } else
+        return 0;
+}
+
+//Calculates the pitch
+float IMUCalc::getPitch( void )
+{
+    //First normalize pitch vector, then calculate the pitch
+    Vector2 pitchVector(top.x, top.z);
+
+    if (pitchVector.Length()>0) {
+        pitchVector = pitchVector.Normalize();
+
+        //if the top is at the bottom, invert the vector
+        if (pitchVector.y<0)
+            pitchVector = -pitchVector;
+        return asin(pitchVector.x);
+
+    } else
+        return 0;
+}
+
+//Calculates the roll
+float IMUCalc::getRoll( void )
+{
+    //First normalize yaw vector, then calculate the heading
+    Vector2 rollVector(top.y, top.z);
+
+    if (rollVector.Length()>0) {
+        rollVector = rollVector.Normalize();
+
+        //check Quadrant
+        if (rollVector.y<0) {
+            if (rollVector.x < 0)
+                return -M_PI - asin(rollVector.x);
+            else
+                return M_PI - asin(rollVector.x);
+        } else {
+            return asin(rollVector.x);
+        }
+    } else
+        return 0;
+}
+
+Vector3 IMUCalc::getGyroOffset( void )
+{
+
+    return gyroOffset;
+}
+
+//The angle between the magnetic vector and the ground vector should be 90 degrees (0.5 pi). We calculate the angle, and rotate the magnetic vector while not changing the angle of
+//the original rotations vector, only we rotate far enough to make it 90 degrees.
+Vector3 IMUCalc::rotateMag(Vector3 magdat, Vector3 ground)
+{
+    //Variables
+    Vector3 retval;
+    Vector3 rotVector;
+    Matrix3x3 rotMatrix;
+    float angle;
+
+    //Calculate the angle between magnetic and acceleration vector
+    rotVector = angleBetween(magdat, ground);
+    angle = rotVector.Length();
+
+    //Calculate how far we have to rotate magnetic vector
+    angle = 0.5 * M_PI - angle;
+
+    //And do that
+    retval = rotateVector(magdat, rotVector, -angle);
+
+    return retval;
+}
+
+
+// Vector calculations not included in GTMath
+
+Vector3 IMUCalc::angleBetween(Vector3 vectorA, Vector3 vectorB)
+{
+
+    
+    
+    float angle;
+    if ((vectorA.Length()==0)||(vectorB.Length()==0))
+        angle=0;
+    else
+        angle = vectorA.Angle(vectorB);
+    // if no noticable rotation is available return zero rotation
+    // this way we avoid Cross product artifacts
+    if( abs(angle) < 0.0001 ) return Vector3( 0, 0, 0);
+    // in this case there are 2 lines on the same axis
+    if(abs(angle-M_PI) < 0.0001) {
+        //They are in opposite directions, rotate one by 90 degrees, that picks one of the infinite amount of rotation angles you get
+        float temp = vectorB.z;
+        vectorB.z=vectorB.y;
+        vectorB.y=vectorB.x;
+        vectorB.x=temp;
+    }
+    Vector3 axis = (vectorA.CrossP(vectorB));
+    axis=axis.Normalize();
+    axis *= (angle);
+
+    return axis;
+}
+
+
+Vector3 IMUCalc::rotateVector(Vector3 vector, Vector3 axis, float angle)
+{
+    if (axis.Length()>0.0001) {
+        Matrix3x3 rotMatrix = Matrix3x3::RotateAxis(axis, angle);
+        return rotMatrix.Transform(vector);
+    }
+    return vector;
+}

--- a/IMUCalc.h	Wed Jul 11 12:26:44 2012 +0000
+++ b/IMUCalc.h	Wed Jan 22 21:19:18 2014 +0000
@@ -21,22 +21,86 @@
 
 class IMUCalc {
 public:
-
+    IMUCalc( void );
+    
+    /**
+     * Calculates new headings
+     *
+     * @param accdat - pointer to float array with length 3 with acceleration data
+     * @param gyrodat - pointer to float array with length 3 with gyroscope data
+     * @param magdat - pointer to float array with length 3 with magnetic data
+     * @param timestep - float with timestep in seconds
+     */
+    void runCalc(float *accdat, float *gyrodat, float *magdat, float timestep);
+    
+    /**
+    * Returns the yaw
+    *
+    * @param return - float with yaw in rads
+    */
+    float getYaw( void );
+    
+    /**
+    * Returns the pitch
+    *
+    * @param return - float with pitch in rads
+    */
+    float getPitch( void );
+    
+    /**
+    * Returns the roll
+    *
+    * @param return - float with roll in rads
+    */
+    float getRoll( void );
+    
+    /**
+    * Returns the calculated offset of the gyro
+    *
+    * @param return - Vector3 with gyro offsets
+    */
+    Vector3 getGyroOffset( void );
 
 
 
 //private:
+    /**
+     * Rotates magnetic vector to be in plane
+     *
+     * @param magdat - Vector3 with magnetic data
+     * @param ground - Vector3 with data which direction the ground is
+     *
+     * @param return - Vector3 with new magnetic vector
+     */
+    Vector3 rotateMag(Vector3 magdat, Vector3 ground);
     
     /**
      * Calculates the angle between two vectors
      *
      * @param vectorA - first vector
      * @param vectorB - second vector
-     * @param return - angle between the two vectors
+     * @param return - vector with which to rotate vectorA to get vectorB
      */
     static Vector3 angleBetween(Vector3 vectorA, Vector3 VectorB);
+    
+    /**
+     * Rotate a vector around an axis
+     *
+     * @param vector - vector to rotate
+     * @param axis - axis to rotate it around
+     * @param angle - angle to rotate the vectors
+     * @param return - resulting vector
+     */
+    static Vector3 rotateVector(Vector3 vector, Vector3 axis, float angle);
 
 
+    Vector3 heading;
+    Vector3 top;
+    Vector3 gyroOffset;
+    
+    float absGain;
+    bool initialRun;
+
 };
 
 #endif
\ No newline at end of file