David Spillman / Mbed 2 deprecated GPSNavigationNew

Crude navigation

Dependencies:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

IMUDataAndFilters.h@17:ba45eaae96b3, 2015-05-09 (annotated)

Committer:
Spilly
Date:
Sat May 09 22:29:01 2015 +0000
Revision:
17:ba45eaae96b3
Parent:
14:fd20b7ac8de8 
Fix for SD card GPS waypoints

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Spilly 8:c77ab7615b21 1 /*************************************************************************************************************************************************************/
Spilly 5:40ac894e0fa7 2 // Created by: Ryan Spillman
Spilly 5:40ac894e0fa7 3 //
Spilly 13:17f04a55c6e2 4 // Last updated 4/29/2015
Spilly 5:40ac894e0fa7 5 //
Spilly 5:40ac894e0fa7 6 // Contains several functions such as calculations for roll, pitch, and yaw (tilt compensated compass)
Spilly 5:40ac894e0fa7 7 // Also contains various filtering and calibration functions
Spilly 5:40ac894e0fa7 8 //
Spilly 5:40ac894e0fa7 9 // Requires L3GD20 gyro and LSM303DLHC magnetometer and accelerometer
Spilly 8:c77ab7615b21 10 /*************************************************************************************************************************************************************/
Spilly 5:40ac894e0fa7 11
Spilly 0:e79311aae7ed 12 #include "mbed.h"
Spilly 0:e79311aae7ed 13 #include "math.h"
Spilly 0:e79311aae7ed 14 #include "L3GD20.h"
Spilly 0:e79311aae7ed 15 #include "LSM303DLHC.h"
Spilly 0:e79311aae7ed 16
Spilly 12:273479524c71 17 #define IMUDataAndFilters_h
Spilly 5:40ac894e0fa7 18
Spilly 14:fd20b7ac8de8 19 L3GD20 gyro(PTE25, PTE24); //L3GD20/L3GD20.cpp
Spilly 14:fd20b7ac8de8 20 LSM303DLHC compass(PTE25, PTE24); //LSM303DLHC/LSM303DLHC.cpp
Spilly 0:e79311aae7ed 21
Spilly 7:ebc76b0f21da 22 #define ALPHA_H 0.2f //Heading alpha (variable for low pass filter)
Spilly 7:ebc76b0f21da 23 #define ALPHA_A 0.1f //Heading accelerometer (variable for low pass filter)
Spilly 7:ebc76b0f21da 24 #define EARTHRADIUS 6378100.000000000000000000f //Radius of the earth in meters (DO NOT MODIFY)
Spilly 7:ebc76b0f21da 25 #define MAGN_PERIOD (1 / 15.0f) //magnetometer polling period (15 Hz) must manually update hardware ODR registers if chaging this value
Spilly 7:ebc76b0f21da 26 #define ACCEL_PERIOD (1 / 50.0f) //accelerometer polling period (50 Hz) must manually update hardware ODR registers if chaging this value
Spilly 7:ebc76b0f21da 27 #define GYRO_SAMPLE_PERIOD 0.01f //gyro sample period in seconds
Spilly 5:40ac894e0fa7 28 #define M_PI 3.1415926535897932384626433832795f
Spilly 5:40ac894e0fa7 29 #define TWO_PI 6.283185307179586476925286766559f
Spilly 5:40ac894e0fa7 30 #define RADTODEGREE 57.295779513082320876798154814105f
Spilly 5:40ac894e0fa7 31 #define DEGREETORAD 0.01745329251994329576923690768489f
Spilly 5:40ac894e0fa7 32 #define GRAVITY 1.0f
Spilly 5:40ac894e0fa7 33 #define CALIBRATION_COUNT 64
Spilly 5:40ac894e0fa7 34 #define AVG_COUNT 10
Spilly 0:e79311aae7ed 35
Spilly 4:a397b44a0fe8 36 // "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
Spilly 5:40ac894e0fa7 37 #define ACCEL_X_MIN -1.000000000f
Spilly 5:40ac894e0fa7 38 #define ACCEL_X_MAX 1.023437500f
Spilly 5:40ac894e0fa7 39 #define ACCEL_Y_MIN -1.015625000f
Spilly 5:40ac894e0fa7 40 #define ACCEL_Y_MAX 1.023437500f
Spilly 5:40ac894e0fa7 41 #define ACCEL_Z_MIN -1.023437500f
Spilly 5:40ac894e0fa7 42 #define ACCEL_Z_MAX 0.960937500f
Spilly 0:e79311aae7ed 43
Spilly 4:a397b44a0fe8 44 // "magn x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
Spilly 5:40ac894e0fa7 45 #define MAGN_X_MIN (-0.370909f)
Spilly 5:40ac894e0fa7 46 #define MAGN_X_MAX (0.569091f)
Spilly 5:40ac894e0fa7 47 #define MAGN_Y_MIN (-0.516364f)
Spilly 5:40ac894e0fa7 48 #define MAGN_Y_MAX (0.392727f)
Spilly 5:40ac894e0fa7 49 #define MAGN_Z_MIN (-0.618367f)
Spilly 5:40ac894e0fa7 50 #define MAGN_Z_MAX (0.408163f)
Spilly 5:40ac894e0fa7 51
Spilly 4:a397b44a0fe8 52 // Sensor calibration scale and offset values
Spilly 4:a397b44a0fe8 53 #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 54 #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 55 #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 56 #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
Spilly 4:a397b44a0fe8 57 #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
Spilly 4:a397b44a0fe8 58 #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
Spilly 4:a397b44a0fe8 59
Spilly 4:a397b44a0fe8 60 #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 61 #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 62 #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
Spilly 5:40ac894e0fa7 63 //not sure what the "normal" value is, thus not using scale for magnetometer
Spilly 5:40ac894e0fa7 64 //#define MAGN_X_SCALE (??? / (MAGN_X_MAX - MAGN_X_OFFSET))
Spilly 5:40ac894e0fa7 65 //#define MAGN_Y_SCALE (??? / (MAGN_Y_MAX - MAGN_Y_OFFSET))
Spilly 5:40ac894e0fa7 66 //#define MAGN_Z_SCALE (??? / (MAGN_Z_MAX - MAGN_Z_OFFSET))
Spilly 4:a397b44a0fe8 67
Spilly 4:a397b44a0fe8 68
Spilly 5:40ac894e0fa7 69 float magnetom[3] = {0,0,0};
Spilly 5:40ac894e0fa7 70 float accel[3] = {0,0,0};
Spilly 5:40ac894e0fa7 71 float degGyro[3] = {0,0,0};
Spilly 5:40ac894e0fa7 72 float prevDegGyro[3]= {0,0,0};
Spilly 5:40ac894e0fa7 73 float gyroOffset[3] = {0,0,0};
Spilly 5:40ac894e0fa7 74 float roll = 0, pitch = 0, yaw = 0;
Spilly 5:40ac894e0fa7 75 float totalAccel = 0;
Spilly 5:40ac894e0fa7 76 float normalizedGravity;
Spilly 0:e79311aae7ed 77
Spilly 5:40ac894e0fa7 78 float lowPassAccel[3] = {0.0000001f,0.0000001f,0.0000001f};
Spilly 0:e79311aae7ed 79
Spilly 5:40ac894e0fa7 80 float prevYaw = 0;
Spilly 7:ebc76b0f21da 81
Spilly 7:ebc76b0f21da 82 /********************************************************************************************************************************/
Spilly 7:ebc76b0f21da 83 // Low Pass
Spilly 7:ebc76b0f21da 84 /********************************************************************************************************************************/
Spilly 7:ebc76b0f21da 85
Spilly 5:40ac894e0fa7 86 float lowPass(float input, float output, int select)
Spilly 5:40ac894e0fa7 87 {
Spilly 8:c77ab7615b21 88 //Magnetometer Heading alpha
Spilly 8:c77ab7615b21 89 if(select == 0)
Spilly 8:c77ab7615b21 90 {
Spilly 8:c77ab7615b21 91 if (output == 0.0000001f) return input;
Spilly 8:c77ab7615b21 92
Spilly 8:c77ab7615b21 93 output = output + ALPHA_H * (input - output);
Spilly 8:c77ab7615b21 94
Spilly 8:c77ab7615b21 95 return output;
Spilly 8:c77ab7615b21 96 }
Spilly 5:40ac894e0fa7 97 //Accelerometer alpha
Spilly 8:c77ab7615b21 98 else if(select == 1)
Spilly 5:40ac894e0fa7 99 {
Spilly 5:40ac894e0fa7 100 if (output == 0.0000001f) return input;
Spilly 5:40ac894e0fa7 101
Spilly 5:40ac894e0fa7 102 output = output + ALPHA_A * (input - output);
Spilly 5:40ac894e0fa7 103
Spilly 5:40ac894e0fa7 104 return output;
Spilly 5:40ac894e0fa7 105 }
Spilly 8:c77ab7615b21 106 //GPS heading alpha
Spilly 5:40ac894e0fa7 107 else
Spilly 5:40ac894e0fa7 108 {
Spilly 5:40ac894e0fa7 109 if (output == 0.0000001f) return input;
Spilly 5:40ac894e0fa7 110
Spilly 8:c77ab7615b21 111 output = output + ALPHA_A * (input - output);
Spilly 5:40ac894e0fa7 112
Spilly 5:40ac894e0fa7 113 return output;
Spilly 5:40ac894e0fa7 114 }
Spilly 5:40ac894e0fa7 115 }
Spilly 0:e79311aae7ed 116
Spilly 0:e79311aae7ed 117 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 118 // getMagn
Spilly 5:40ac894e0fa7 119 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 120
Spilly 5:40ac894e0fa7 121 void getMagn()
Spilly 5:40ac894e0fa7 122 {
Spilly 12:273479524c71 123 //LSM303DLHC/LSM303DLHC.cpp
Spilly 5:40ac894e0fa7 124 compass.ReadMagnOnly(&magnetom[0], &magnetom[1], &magnetom[2]);
Spilly 5:40ac894e0fa7 125
Spilly 5:40ac894e0fa7 126 //Compensate magnetometer error
Spilly 8:c77ab7615b21 127 //magnetom[0] -= MAGN_X_OFFSET;
Spilly 8:c77ab7615b21 128 //magnetom[1] -= MAGN_Y_OFFSET;
Spilly 8:c77ab7615b21 129 //magnetom[2] -= MAGN_Z_OFFSET;
Spilly 5:40ac894e0fa7 130 }
Spilly 5:40ac894e0fa7 131 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 132 // getAccel
Spilly 0:e79311aae7ed 133 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 134
Spilly 5:40ac894e0fa7 135 void getAccel()
Spilly 0:e79311aae7ed 136 {
Spilly 12:273479524c71 137 //LSM303DLHC/LSM303DLHC.cpp
Spilly 5:40ac894e0fa7 138 compass.ReadAccOnly(&accel[0], &accel[1], &accel[2]);
Spilly 4:a397b44a0fe8 139
Spilly 4:a397b44a0fe8 140 //Compensate accelerometer error
Spilly 12:273479524c71 141 //TODO: verify compensation calculations
Spilly 5:40ac894e0fa7 142 accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
Spilly 5:40ac894e0fa7 143 accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
Spilly 5:40ac894e0fa7 144 accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
Spilly 5:40ac894e0fa7 145
Spilly 4:a397b44a0fe8 146
Spilly 5:40ac894e0fa7 147 for(int i = 0; i <= 3; i++)
Spilly 5:40ac894e0fa7 148 {
Spilly 5:40ac894e0fa7 149 lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
Spilly 5:40ac894e0fa7 150 }
Spilly 0:e79311aae7ed 151 }
Spilly 7:ebc76b0f21da 152
Spilly 7:ebc76b0f21da 153 /********************************************************************************************************************************/
Spilly 7:ebc76b0f21da 154 // Integrate Yaw With Gyro Data
Spilly 7:ebc76b0f21da 155 /********************************************************************************************************************************/
Spilly 12:273479524c71 156 //not used
Spilly 5:40ac894e0fa7 157 void gyroIntegral()
Spilly 5:40ac894e0fa7 158 {
Spilly 5:40ac894e0fa7 159 float yawDiff = yaw - prevYaw;
Spilly 5:40ac894e0fa7 160 float absYawDiff = sqrt(yawDiff * yawDiff);
Spilly 5:40ac894e0fa7 161 //vC[x] = vP[x] + ((aP[x] + ((inertialAccel[x] - aP[x]) / 2)) * deltaT);
Spilly 7:ebc76b0f21da 162 float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * MAGN_PERIOD);
Spilly 5:40ac894e0fa7 163 float absGyroInt = sqrt(gyroInt * gyroInt);
Spilly 5:40ac894e0fa7 164 prevDegGyro[2] = degGyro[2];
Spilly 5:40ac894e0fa7 165 if(absYawDiff > absGyroInt)
Spilly 5:40ac894e0fa7 166 {
Spilly 5:40ac894e0fa7 167 yaw = prevYaw + gyroInt;
Spilly 5:40ac894e0fa7 168 }
Spilly 5:40ac894e0fa7 169 }
Spilly 7:ebc76b0f21da 170
Spilly 0:e79311aae7ed 171 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 172 // Compass Heading
Spilly 0:e79311aae7ed 173 /********************************************************************************************************************************/
Spilly 12:273479524c71 174 //see http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf for more info on these formulas
Spilly 0:e79311aae7ed 175 void Compass_Heading()
Spilly 0:e79311aae7ed 176 {
Spilly 5:40ac894e0fa7 177 float mag_x;
Spilly 5:40ac894e0fa7 178 float mag_y;
Spilly 7:ebc76b0f21da 179 float cos_roll;
Spilly 7:ebc76b0f21da 180 float sin_roll;
Spilly 7:ebc76b0f21da 181 float cos_pitch;
Spilly 7:ebc76b0f21da 182 float sin_pitch;
Spilly 7:ebc76b0f21da 183
Spilly 7:ebc76b0f21da 184 //saves a few processor cycles by calculating and storing values
Spilly 7:ebc76b0f21da 185 cos_roll = cos(roll);
Spilly 7:ebc76b0f21da 186 sin_roll = sin(roll);
Spilly 7:ebc76b0f21da 187 cos_pitch = cos(pitch);
Spilly 7:ebc76b0f21da 188 sin_pitch = sin(pitch);
Spilly 7:ebc76b0f21da 189
Spilly 7:ebc76b0f21da 190 // Tilt compensated magnetic field X
Spilly 7:ebc76b0f21da 191 mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
Spilly 12:273479524c71 192
Spilly 7:ebc76b0f21da 193 // Tilt compensated magnetic field Y
Spilly 7:ebc76b0f21da 194 mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
Spilly 7:ebc76b0f21da 195
Spilly 5:40ac894e0fa7 196 // Magnetic Heading
Spilly 5:40ac894e0fa7 197 yaw = atan2(-mag_x, mag_y);
Spilly 12:273479524c71 198
Spilly 5:40ac894e0fa7 199 //make negative angles positive
Spilly 5:40ac894e0fa7 200 if(yaw < 0) yaw = yaw + TWO_PI;
Spilly 5:40ac894e0fa7 201 //yaw = yaw + M_PI;
Spilly 5:40ac894e0fa7 202 yaw = yaw * RADTODEGREE;
Spilly 0:e79311aae7ed 203 }
Spilly 0:e79311aae7ed 204
Spilly 0:e79311aae7ed 205 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 206 // getAttitude
Spilly 0:e79311aae7ed 207 /********************************************************************************************************************************/
Spilly 12:273479524c71 208 //see http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf for more info on these formulas
Spilly 0:e79311aae7ed 209 void getAttitude()
Spilly 0:e79311aae7ed 210 {
Spilly 7:ebc76b0f21da 211 float temp1[3];
Spilly 7:ebc76b0f21da 212 float temp2[3];
Spilly 7:ebc76b0f21da 213 float xAxis[3] = {1.0f, 0.0f, 0.0f};
Spilly 7:ebc76b0f21da 214
Spilly 7:ebc76b0f21da 215 // GET PITCH
Spilly 7:ebc76b0f21da 216 // Using y-z-plane-component/x-component of gravity vector
Spilly 7:ebc76b0f21da 217 pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
Spilly 7:ebc76b0f21da 218
Spilly 7:ebc76b0f21da 219 //printf("P = %f", pitch);
Spilly 5:40ac894e0fa7 220
Spilly 7:ebc76b0f21da 221 // GET ROLL
Spilly 7:ebc76b0f21da 222 // Compensate pitch of gravity vector
Spilly 7:ebc76b0f21da 223 temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
Spilly 7:ebc76b0f21da 224 temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
Spilly 7:ebc76b0f21da 225 temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
Spilly 5:40ac894e0fa7 226
Spilly 7:ebc76b0f21da 227 temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
Spilly 7:ebc76b0f21da 228 temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
Spilly 7:ebc76b0f21da 229 temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
Spilly 7:ebc76b0f21da 230
Spilly 7:ebc76b0f21da 231 // Since we compensated for pitch, x-z-plane-component equals z-component:
Spilly 7:ebc76b0f21da 232 roll = atan2(temp2[1], temp2[2]);
Spilly 5:40ac894e0fa7 233 }
Spilly 5:40ac894e0fa7 234
Spilly 5:40ac894e0fa7 235 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 236 // Filtered_Attitude()
Spilly 5:40ac894e0fa7 237 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 238
Spilly 5:40ac894e0fa7 239 void Filtered_Attitude()
Spilly 5:40ac894e0fa7 240 {
Spilly 7:ebc76b0f21da 241 float temp1[3];
Spilly 7:ebc76b0f21da 242 float temp2[3];
Spilly 7:ebc76b0f21da 243 float xAxis[3] = {1.0f, 0.0f, 0.0f};
Spilly 7:ebc76b0f21da 244
Spilly 7:ebc76b0f21da 245 // GET PITCH
Spilly 7:ebc76b0f21da 246 // Using y-z-plane-component/x-component of gravity vector
Spilly 7:ebc76b0f21da 247 pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
Spilly 7:ebc76b0f21da 248
Spilly 7:ebc76b0f21da 249 //printf("P = %f", pitch);
Spilly 7:ebc76b0f21da 250
Spilly 7:ebc76b0f21da 251 // GET ROLL
Spilly 7:ebc76b0f21da 252 // Compensate pitch of gravity vector
Spilly 7:ebc76b0f21da 253 temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
Spilly 7:ebc76b0f21da 254 temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
Spilly 7:ebc76b0f21da 255 temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
Spilly 7:ebc76b0f21da 256
Spilly 7:ebc76b0f21da 257 temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
Spilly 7:ebc76b0f21da 258 temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
Spilly 7:ebc76b0f21da 259 temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
Spilly 7:ebc76b0f21da 260
Spilly 7:ebc76b0f21da 261 // Since we compensated for pitch, x-z-plane-component equals z-component:
Spilly 7:ebc76b0f21da 262 roll = atan2(temp2[1], temp2[2]);
Spilly 0:e79311aae7ed 263 }
Spilly 0:e79311aae7ed 264
Spilly 5:40ac894e0fa7 265 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 266 // updateAngles
Spilly 5:40ac894e0fa7 267 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 268
Spilly 5:40ac894e0fa7 269 void updateAngles()
Spilly 0:e79311aae7ed 270 {
Spilly 5:40ac894e0fa7 271 Filtered_Attitude();
Spilly 5:40ac894e0fa7 272 Compass_Heading();
Spilly 5:40ac894e0fa7 273 }
Spilly 5:40ac894e0fa7 274
Spilly 5:40ac894e0fa7 275 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 276 // normalizeGravity
Spilly 5:40ac894e0fa7 277 /********************************************************************************************************************************/
Spilly 12:273479524c71 278 //not used
Spilly 5:40ac894e0fa7 279 void normalizeGravity()
Spilly 5:40ac894e0fa7 280 {
Spilly 5:40ac894e0fa7 281 float accumulator = 0;
Spilly 5:40ac894e0fa7 282 int sampleCount = 0;
Spilly 5:40ac894e0fa7 283
Spilly 5:40ac894e0fa7 284
Spilly 5:40ac894e0fa7 285 //We'll take a certain number of samples and then average them to calculate the average
Spilly 5:40ac894e0fa7 286 while (sampleCount < AVG_COUNT)
Spilly 5:40ac894e0fa7 287 {
Spilly 5:40ac894e0fa7 288 getAccel();
Spilly 5:40ac894e0fa7 289 //add current sample to previous samples
Spilly 5:40ac894e0fa7 290 accumulator += sqrt((accel[0] * accel[0]) + (accel[1] * accel[1]) + (accel[2] * accel[2]));
Spilly 5:40ac894e0fa7 291 sampleCount++;
Spilly 5:40ac894e0fa7 292 //Make sure the IMU has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 293 wait(0.06);
Spilly 5:40ac894e0fa7 294 }
Spilly 5:40ac894e0fa7 295
Spilly 5:40ac894e0fa7 296
Spilly 5:40ac894e0fa7 297 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 298 normalizedGravity = accumulator / AVG_COUNT;
Spilly 5:40ac894e0fa7 299
Spilly 5:40ac894e0fa7 300 }
Spilly 5:40ac894e0fa7 301 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 302 // gyroAvg
Spilly 5:40ac894e0fa7 303 /********************************************************************************************************************************/
Spilly 12:273479524c71 304 //not used
Spilly 5:40ac894e0fa7 305 void gyroAvg()
Spilly 2:503a5ac6c3b6 306 {
Spilly 5:40ac894e0fa7 307 float accumulator[3] = {0,0,0};
Spilly 5:40ac894e0fa7 308 int sampleCount = 0;
Spilly 5:40ac894e0fa7 309
Spilly 5:40ac894e0fa7 310
Spilly 5:40ac894e0fa7 311 //We'll take a certain number of samples and then average them to calculate the offset
Spilly 5:40ac894e0fa7 312 while (sampleCount < AVG_COUNT)
Spilly 5:40ac894e0fa7 313 {
Spilly 5:40ac894e0fa7 314 //get gyro data
Spilly 5:40ac894e0fa7 315 gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
Spilly 5:40ac894e0fa7 316 //add current sample to previous samples
Spilly 5:40ac894e0fa7 317 accumulator[2] += degGyro[2];
Spilly 5:40ac894e0fa7 318 sampleCount++;
Spilly 5:40ac894e0fa7 319 //Make sure the gyro has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 320 wait(GYRO_SAMPLE_PERIOD);
Spilly 5:40ac894e0fa7 321 }
Spilly 5:40ac894e0fa7 322
Spilly 5:40ac894e0fa7 323
Spilly 5:40ac894e0fa7 324 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 325 degGyro[2] = accumulator[2] / AVG_COUNT;
Spilly 5:40ac894e0fa7 326
Spilly 5:40ac894e0fa7 327 }
Spilly 5:40ac894e0fa7 328
Spilly 5:40ac894e0fa7 329 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 330 // gyroCal
Spilly 5:40ac894e0fa7 331 /********************************************************************************************************************************/
Spilly 12:273479524c71 332 //not used
Spilly 5:40ac894e0fa7 333 void gyroCal()
Spilly 5:40ac894e0fa7 334 {
Spilly 5:40ac894e0fa7 335 float accumulator[3] = {0,0,0};
Spilly 5:40ac894e0fa7 336 int sampleCount = 0;
Spilly 5:40ac894e0fa7 337
Spilly 5:40ac894e0fa7 338
Spilly 5:40ac894e0fa7 339 //We'll take a certain number of samples and then average them to calculate the offset
Spilly 5:40ac894e0fa7 340 while (sampleCount < CALIBRATION_COUNT)
Spilly 5:40ac894e0fa7 341 {
Spilly 5:40ac894e0fa7 342 float gyroData[3];
Spilly 5:40ac894e0fa7 343 //Make sure the gyro has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 344 wait(GYRO_SAMPLE_PERIOD);
Spilly 5:40ac894e0fa7 345 //get gyro data
Spilly 5:40ac894e0fa7 346 gyro.read(&gyroData[0],&gyroData[1],&gyroData[2]);
Spilly 5:40ac894e0fa7 347 for(int i = 0; i < 3; i++)
Spilly 5:40ac894e0fa7 348 {
Spilly 5:40ac894e0fa7 349 //add current sample to previous samples
Spilly 5:40ac894e0fa7 350 accumulator[i] += gyroData[i];
Spilly 5:40ac894e0fa7 351 }
Spilly 5:40ac894e0fa7 352 sampleCount++;
Spilly 5:40ac894e0fa7 353 }
Spilly 5:40ac894e0fa7 354
Spilly 5:40ac894e0fa7 355 for(int i = 0; i < 3; i++)
Spilly 5:40ac894e0fa7 356 {
Spilly 5:40ac894e0fa7 357 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 358 gyroOffset[i] = accumulator[i] / CALIBRATION_COUNT;
Spilly 5:40ac894e0fa7 359 }
Spilly 5:40ac894e0fa7 360 }
Spilly 5:40ac894e0fa7 361
Spilly 7:ebc76b0f21da 362 /********************************************************************************************************************************/
Spilly 7:ebc76b0f21da 363 // Only Get Gyro Data
Spilly 7:ebc76b0f21da 364 /********************************************************************************************************************************/
Spilly 12:273479524c71 365 //not used
Spilly 5:40ac894e0fa7 366 void gyroOnly()
Spilly 5:40ac894e0fa7 367 {
Spilly 5:40ac894e0fa7 368 gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
Spilly 5:40ac894e0fa7 369 //compensate for gyro offset
Spilly 5:40ac894e0fa7 370 for(int i=0;i<=3;i++)
Spilly 5:40ac894e0fa7 371 {
Spilly 5:40ac894e0fa7 372 degGyro[i] -= gyroOffset[i];
Spilly 5:40ac894e0fa7 373 }
Spilly 7:ebc76b0f21da 374 }