Diff: modSensData.h

Revision:

7:ebc76b0f21da

Parent:

5:40ac894e0fa7

Child:

8:c77ab7615b21

--- a/modSensData.h	Sun Apr 05 01:50:25 2015 +0000
+++ b/modSensData.h	Tue Apr 07 03:49:50 2015 +0000
@@ -19,20 +19,19 @@
 L3GD20 gyro(PTE25, PTE24);
 LSM303DLHC compass(PTE25, PTE24);
 
-//Variable for low pass filter
-#define ALPHA_H             0.2f 
-#define ALPHA_A             0.1f
-#define MAX_DELTA_HEADING   20
-
+#define ARRIVED             4.0f                        //Tolerance, in meters, for when goal location is reached
+#define ALPHA_H             0.2f                        //Heading alpha (variable for low pass filter)
+#define ALPHA_A             0.1f                        //Heading accelerometer (variable for low pass filter)
+#define EARTHRADIUS     6378100.000000000000000000f     //Radius of the earth in meters (DO NOT MODIFY)
+#define MAGN_PERIOD     (1 / 15.0f)                     //magnetometer polling period (15 Hz) must manually update hardware ODR registers if chaging this value
+#define ACCEL_PERIOD    (1 / 50.0f)                     //accelerometer polling period (50 Hz) must manually update hardware ODR registers if chaging this value
+#define GYRO_SAMPLE_PERIOD  0.01f                       //gyro sample period in seconds
+#define GPS_PERIOD      (1 / 5.0f)                      //GPS polling period (5 Hz)
 #define M_PI                3.1415926535897932384626433832795f
 #define TWO_PI              6.283185307179586476925286766559f
 #define RADTODEGREE         57.295779513082320876798154814105f
 #define DEGREETORAD         0.01745329251994329576923690768489f
 #define GRAVITY             1.0f
-#define THRESHOLD           0.02f
-
-//gyro sample period in seconds
-#define GYRO_SAMPLE_PERIOD  0.01f
 #define CALIBRATION_COUNT   64
 #define AVG_COUNT           10
 
@@ -52,12 +51,6 @@
 #define MAGN_Z_MIN          (-0.618367f)
 #define MAGN_Z_MAX          (0.408163f)
 
-
-//Period in seconds of heading PID loop
-#define COMPASS_PERIOD              0.060f
-
-#define YAW_VAR_TOLER               5.0f
-
 // Sensor calibration scale and offset values
 #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
 #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
@@ -84,24 +77,14 @@
 float totalAccel = 0;
 float normalizedGravity;
 
-float filteredYaw, filteredPitch, filteredRoll;
-
 float lowPassAccel[3] = {0.0000001f,0.0000001f,0.0000001f};
-float lowPassMagnetom[3] = {0.0000001f,0.0000001f,0.0000001f};
 
 float prevYaw = 0;
-int cycleCount = 0;
-int firstPass = 1;
-/*
-float lowPass(float input, float output)  
-{
-    if (output == 0.0000001f) return input;
-    
-    output = (ALPHA * input) + ((1 - ALPHA) * output);
-    
-    return output;
-}
-*/
+
+/********************************************************************************************************************************/
+//                                          Low Pass
+/********************************************************************************************************************************/
+
 float lowPass(float input, float output, int select) 
 {
     //Accelerometer alpha
@@ -156,12 +139,17 @@
         lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
     }    
 }
+
+/********************************************************************************************************************************/
+//                                                      Integrate Yaw With Gyro Data
+/********************************************************************************************************************************/
+
 void gyroIntegral()
 {
     float yawDiff = yaw - prevYaw;
     float absYawDiff = sqrt(yawDiff * yawDiff);
     //vC[x] = vP[x] + ((aP[x] + ((inertialAccel[x] - aP[x]) / 2)) * deltaT);
-    float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * COMPASS_PERIOD);
+    float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * MAGN_PERIOD);
     float absGyroInt = sqrt(gyroInt * gyroInt);
     prevDegGyro[2] = degGyro[2];
     if(absYawDiff > absGyroInt)
@@ -169,6 +157,7 @@
         yaw = prevYaw + gyroInt;
     }
 }
+
 /********************************************************************************************************************************/
 //                                                      Compass Heading
 /********************************************************************************************************************************/
@@ -178,37 +167,27 @@
 
     float mag_x;
     float mag_y;
-    //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
-    //{
-        float cos_roll;
-        float sin_roll;
-        float cos_pitch;
-        float sin_pitch;
-        
-        //saves a few processor cycles by calculating and storing values
-        cos_roll = cos(roll);
-        sin_roll = sin(roll);
-        cos_pitch = cos(pitch);
-        sin_pitch = sin(pitch);
-        
-        // Tilt compensated magnetic field X
-        //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
-        mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
-        // Tilt compensated magnetic field Y
-        //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
-        mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
-    //}
-    /*  
-    else
-    {
-        mag_x = magnetom[0] ;
-        mag_y = magnetom[1];
-    }
-    */
+    float cos_roll;
+    float sin_roll;
+    float cos_pitch;
+    float sin_pitch;
+    
+    //saves a few processor cycles by calculating and storing values
+    cos_roll = cos(roll);
+    sin_roll = sin(roll);
+    cos_pitch = cos(pitch);
+    sin_pitch = sin(pitch);
+    
+    // Tilt compensated magnetic field X
+    //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
+    mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
+    // Tilt compensated magnetic field Y
+    //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+    mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+
     // Magnetic Heading
     yaw = atan2(-mag_x, mag_y);
-    
-    
+        
     //make negative angles positive
     if(yaw < 0) yaw = yaw + TWO_PI;
     //yaw = yaw + M_PI;
@@ -221,74 +200,28 @@
 
 void getAttitude()
 {
-  float temp1[3];
-  float temp2[3];
-  float xAxis[3] = {1.0f, 0.0f, 0.0f};
-  
-  // GET PITCH
-  // Using y-z-plane-component/x-component of gravity vector    
-  pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
-  
-  //printf("P = %f", pitch);
-  
-  // GET ROLL
-  // Compensate pitch of gravity vector 
-  temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
-  temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
-  temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
-  
-  temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
-  temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
-  temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
-  
-  // Since we compensated for pitch, x-z-plane-component equals z-component:
-  roll = atan2(temp2[1], temp2[2]);
-}
-
-/********************************************************************************************************************************/
-//                                                      Compass Heading
-/********************************************************************************************************************************/
-
-void Filtered_Compass_Heading()
-{
-
-    float mag_x;
-    float mag_y;
-    //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
-    //{
-        float cos_roll;
-        float sin_roll;
-        float cos_pitch;
-        float sin_pitch;
-        
-        //saves a few processor cycles by calculating and storing values
-        cos_roll = cos(filteredRoll);
-        sin_roll = sin(filteredRoll);
-        cos_pitch = cos(filteredPitch);
-        sin_pitch = sin(filteredPitch);
-        
-        // Tilt compensated magnetic field X
-        //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
-        mag_x = lowPassMagnetom[0] * cos_pitch + lowPassMagnetom[1] * sin_roll * sin_pitch + lowPassMagnetom[2] * cos_roll * sin_pitch;
-        // Tilt compensated magnetic field Y
-        //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
-        mag_y = lowPassMagnetom[1] * cos_roll - lowPassMagnetom[2] * sin_roll;
-    //}
-    /*  
-    else
-    {
-        mag_x = magnetom[0] ;
-        mag_y = magnetom[1];
-    }
-    */
-    // Magnetic Heading
-    filteredYaw = atan2(-mag_x, mag_y);
+    float temp1[3];
+    float temp2[3];
+    float xAxis[3] = {1.0f, 0.0f, 0.0f};
+    
+    // GET PITCH
+    // Using y-z-plane-component/x-component of gravity vector    
+    pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
+    
+    //printf("P = %f", pitch);
     
+    // GET ROLL
+    // Compensate pitch of gravity vector 
+    temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
+    temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
+    temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
     
-    //make negative angles positive
-    if(filteredYaw < 0) filteredYaw = filteredYaw + TWO_PI;
-    //yaw = yaw + M_PI;
-    filteredYaw = filteredYaw * RADTODEGREE;
+    temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
+    temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
+    temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
+    
+    // Since we compensated for pitch, x-z-plane-component equals z-component:
+    roll = atan2(temp2[1], temp2[2]);
 }
 
 /********************************************************************************************************************************/
@@ -297,28 +230,28 @@
 
 void Filtered_Attitude()
 {
-  float temp1[3];
-  float temp2[3];
-  float xAxis[3] = {1.0f, 0.0f, 0.0f};
-  
-  // GET PITCH
-  // Using y-z-plane-component/x-component of gravity vector    
-  pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
-  
-  //printf("P = %f", pitch);
-  
-  // GET ROLL
-  // Compensate pitch of gravity vector 
-  temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
-  temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
-  temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
-  
-  temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
-  temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
-  temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
-  
-  // Since we compensated for pitch, x-z-plane-component equals z-component:
-  roll = atan2(temp2[1], temp2[2]);
+    float temp1[3];
+    float temp2[3];
+    float xAxis[3] = {1.0f, 0.0f, 0.0f};
+    
+    // GET PITCH
+    // Using y-z-plane-component/x-component of gravity vector    
+    pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
+    
+    //printf("P = %f", pitch);
+    
+    // GET ROLL
+    // Compensate pitch of gravity vector 
+    temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
+    temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
+    temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
+    
+    temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
+    temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
+    temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
+    
+    // Since we compensated for pitch, x-z-plane-component equals z-component:
+    roll = atan2(temp2[1], temp2[2]);
 }
 
 
@@ -329,132 +262,11 @@
 void updateAngles()
 //int updateAngles()
 {
-    //readIMU();
-    //getAttitude();
     Filtered_Attitude();
     Compass_Heading();
-    //Filtered_Attitude();
-    //Filtered_Compass_Heading();
-    /*
-    totalAccel = sqrt((lowPassAccel[0] * lowPassAccel[0]) + (lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2]));
-    
-    prevYaw = yaw;
-    getAttitude();
-    Compass_Heading();
-    
-    if((sqrt((prevYaw - yaw) * (prevYaw - yaw)) >= YAW_VAR_TOLER) && (firstPass != 1))
-    {
-        yaw = prevYaw;
-    }
-    else if(firstPass == 1)
-    {
-        firstPass = 0;
-    }
-    
-    //if((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * accel[2]) <= THRESHOLD))
-    if((totalAccel <= (normalizedGravity + THRESHOLD)) && (totalAccel >= (normalizedGravity - THRESHOLD)))
-    {
-        //float prevYaw = yaw;
-        getAttitude();
-        Compass_Heading();
-        //float absOfYawDiff = sqrt((prevYaw - yaw) * (prevYaw - yaw));    
-        //check for noise
-        //if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
-        //{
-        return 1;
-        //}
-        //return 0;
-        
-    }
-    */
-    //return 0;
-    //else Compass_Heading();
-    //else return 0;
-    //getAttitude();
-    //Compass_Heading();
-    //gyroIntegral();
 }
 
 /********************************************************************************************************************************/
-//                                                      magAvg
-/********************************************************************************************************************************/
-
-void compassAvg()
-{
-    float accumulator = 0;
-    //float prevYaw[AVG_COUNT];
-    int sampleCount  = 0;
-
-    
-    //We'll take a certain number of samples and then average them to calculate the offset
-    while (sampleCount < AVG_COUNT) 
-    {
-        updateAngles();
-        
-        //add current sample to previous samples
-        accumulator += yaw;
-        
-        sampleCount++;
-        //Make sure the gyro has had enough time to take a new sample.
-        wait(COMPASS_PERIOD);
-    }
-    
-        //divide by number of samples to get average offset
-    yaw = accumulator / AVG_COUNT;
-    
-
-}
-
-/********************************************************************************************************************************/
-//                                                      magLowPass
-/********************************************************************************************************************************/
-void compLowPass()
-{
-    /*
-    prevMagnetom[5][3];
-    //read sensors five times
-    for(int x = 0; x<= 5; i++)
-    {
-        for(int i = 0; i<=3; i++)
-        {
-            prevMagnetom[x, i] = magnetom[i];
-        }
-        readIMU();
-    }
-    float diff[5][3];
-    for(int x = 1; x<= 5; i++)
-    {
-        sqrt(prevMagnetom[x][0]
-        for(int i = 0; i<=3; i++)
-        {
-            if(diff[x][i] = sqrt((prevMagnetom[x][i] - prevMagnemtom[x - 1]) * (prevMagnetom[x][i] - prevMagnemtom[x - 1])) >= MAG_VAR_TOLER)
-            {
-                
-            }
-        }
-    }
-    
-    float deltaHeading = 0;
-    updateAngles();
-    deltaHeading = sqrt((yaw - prevYaw[cycleCount]) * (yaw - prevYaw[cycleCount]));
-    
-    if(deltaHeading >= MAX_DELTA_HEADING)
-    {
-        yaw = (yaw + prevYaw[cycleCount]) / 2.0f;
-    }
-    cycleCount = cycleCount + 1;
-    prevYaw[cycleCount] = yaw;
-    
-    if(cycleCount >= 4)
-    {
-        cycleCount = 0;
-        prevYaw = {0,0,0,0,0}
-    }
-*/
-}
-
-
-/********************************************************************************************************************************/
 //                                                      normalizeGravity
 /********************************************************************************************************************************/
 
@@ -541,6 +353,10 @@
     }
 }
 
+/********************************************************************************************************************************/
+//                                                      Only Get Gyro Data
+/********************************************************************************************************************************/
+
 void gyroOnly()
 {
     gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
@@ -549,5 +365,4 @@
     {
         degGyro[i] -= gyroOffset[i];
     }
-}
-
+}
\ No newline at end of file