David Spillman / Mbed 2 deprecated GPSNavigationNew

Crude navigation

Dependencies:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

modSensData.h@7:ebc76b0f21da, 2015-04-07 (annotated)

Committer:
Spilly
Date:
Tue Apr 07 03:49:50 2015 +0000
Revision:
7:ebc76b0f21da
Parent:
5:40ac894e0fa7
Child:
8:c77ab7615b21
Added documentation/comments

Who changed what in which revision?

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