David Spillman / Mbed 2 deprecated GPSNavigationNew

Crude navigation

Dependencies:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

modSensData.h@8:c77ab7615b21, 2015-04-27 (annotated)

Committer:
Spilly
Date:
Mon Apr 27 16:49:48 2015 +0000
Revision:
8:c77ab7615b21
Parent:
7:ebc76b0f21da 
BoatProject

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 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 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 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 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 5:40ac894e0fa7 123 compass.ReadMagnOnly(&magnetom[0], &magnetom[1], &magnetom[2]);
Spilly 5:40ac894e0fa7 124
Spilly 5:40ac894e0fa7 125 //Compensate magnetometer error
Spilly 8:c77ab7615b21 126 //magnetom[0] -= MAGN_X_OFFSET;
Spilly 8:c77ab7615b21 127 //magnetom[1] -= MAGN_Y_OFFSET;
Spilly 8:c77ab7615b21 128 //magnetom[2] -= MAGN_Z_OFFSET;
Spilly 5:40ac894e0fa7 129 }
Spilly 5:40ac894e0fa7 130 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 131 // getAccel
Spilly 0:e79311aae7ed 132 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 133
Spilly 5:40ac894e0fa7 134 void getAccel()
Spilly 0:e79311aae7ed 135 {
Spilly 5:40ac894e0fa7 136 compass.ReadAccOnly(&accel[0], &accel[1], &accel[2]);
Spilly 4:a397b44a0fe8 137
Spilly 4:a397b44a0fe8 138 //Compensate accelerometer error
Spilly 5:40ac894e0fa7 139 accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
Spilly 5:40ac894e0fa7 140 accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
Spilly 5:40ac894e0fa7 141 accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
Spilly 5:40ac894e0fa7 142
Spilly 4:a397b44a0fe8 143
Spilly 5:40ac894e0fa7 144 for(int i = 0; i <= 3; i++)
Spilly 5:40ac894e0fa7 145 {
Spilly 5:40ac894e0fa7 146 lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
Spilly 5:40ac894e0fa7 147 }
Spilly 0:e79311aae7ed 148 }
Spilly 7:ebc76b0f21da 149
Spilly 7:ebc76b0f21da 150 /********************************************************************************************************************************/
Spilly 7:ebc76b0f21da 151 // Integrate Yaw With Gyro Data
Spilly 7:ebc76b0f21da 152 /********************************************************************************************************************************/
Spilly 7:ebc76b0f21da 153
Spilly 5:40ac894e0fa7 154 void gyroIntegral()
Spilly 5:40ac894e0fa7 155 {
Spilly 5:40ac894e0fa7 156 float yawDiff = yaw - prevYaw;
Spilly 5:40ac894e0fa7 157 float absYawDiff = sqrt(yawDiff * yawDiff);
Spilly 5:40ac894e0fa7 158 //vC[x] = vP[x] + ((aP[x] + ((inertialAccel[x] - aP[x]) / 2)) * deltaT);
Spilly 7:ebc76b0f21da 159 float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * MAGN_PERIOD);
Spilly 5:40ac894e0fa7 160 float absGyroInt = sqrt(gyroInt * gyroInt);
Spilly 5:40ac894e0fa7 161 prevDegGyro[2] = degGyro[2];
Spilly 5:40ac894e0fa7 162 if(absYawDiff > absGyroInt)
Spilly 5:40ac894e0fa7 163 {
Spilly 5:40ac894e0fa7 164 yaw = prevYaw + gyroInt;
Spilly 5:40ac894e0fa7 165 }
Spilly 5:40ac894e0fa7 166 }
Spilly 7:ebc76b0f21da 167
Spilly 0:e79311aae7ed 168 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 169 // Compass Heading
Spilly 0:e79311aae7ed 170 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 171
Spilly 0:e79311aae7ed 172 void Compass_Heading()
Spilly 0:e79311aae7ed 173 {
Spilly 5:40ac894e0fa7 174
Spilly 5:40ac894e0fa7 175 float mag_x;
Spilly 5:40ac894e0fa7 176 float mag_y;
Spilly 7:ebc76b0f21da 177 float cos_roll;
Spilly 7:ebc76b0f21da 178 float sin_roll;
Spilly 7:ebc76b0f21da 179 float cos_pitch;
Spilly 7:ebc76b0f21da 180 float sin_pitch;
Spilly 7:ebc76b0f21da 181
Spilly 7:ebc76b0f21da 182 //saves a few processor cycles by calculating and storing values
Spilly 7:ebc76b0f21da 183 cos_roll = cos(roll);
Spilly 7:ebc76b0f21da 184 sin_roll = sin(roll);
Spilly 7:ebc76b0f21da 185 cos_pitch = cos(pitch);
Spilly 7:ebc76b0f21da 186 sin_pitch = sin(pitch);
Spilly 7:ebc76b0f21da 187
Spilly 7:ebc76b0f21da 188 // Tilt compensated magnetic field X
Spilly 7:ebc76b0f21da 189 //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
Spilly 7:ebc76b0f21da 190 mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
Spilly 7:ebc76b0f21da 191 // Tilt compensated magnetic field Y
Spilly 7:ebc76b0f21da 192 //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
Spilly 7:ebc76b0f21da 193 mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
Spilly 7:ebc76b0f21da 194
Spilly 5:40ac894e0fa7 195 // Magnetic Heading
Spilly 5:40ac894e0fa7 196 yaw = atan2(-mag_x, mag_y);
Spilly 7:ebc76b0f21da 197
Spilly 5:40ac894e0fa7 198 //make negative angles positive
Spilly 5:40ac894e0fa7 199 if(yaw < 0) yaw = yaw + TWO_PI;
Spilly 5:40ac894e0fa7 200 //yaw = yaw + M_PI;
Spilly 5:40ac894e0fa7 201 yaw = yaw * RADTODEGREE;
Spilly 0:e79311aae7ed 202 }
Spilly 0:e79311aae7ed 203
Spilly 0:e79311aae7ed 204 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 205 // getAttitude
Spilly 0:e79311aae7ed 206 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 207
Spilly 0:e79311aae7ed 208 void getAttitude()
Spilly 0:e79311aae7ed 209 {
Spilly 7:ebc76b0f21da 210 float temp1[3];
Spilly 7:ebc76b0f21da 211 float temp2[3];
Spilly 7:ebc76b0f21da 212 float xAxis[3] = {1.0f, 0.0f, 0.0f};
Spilly 7:ebc76b0f21da 213
Spilly 7:ebc76b0f21da 214 // GET PITCH
Spilly 7:ebc76b0f21da 215 // Using y-z-plane-component/x-component of gravity vector
Spilly 7:ebc76b0f21da 216 pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
Spilly 7:ebc76b0f21da 217
Spilly 7:ebc76b0f21da 218 //printf("P = %f", pitch);
Spilly 5:40ac894e0fa7 219
Spilly 7:ebc76b0f21da 220 // GET ROLL
Spilly 7:ebc76b0f21da 221 // Compensate pitch of gravity vector
Spilly 7:ebc76b0f21da 222 temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
Spilly 7:ebc76b0f21da 223 temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
Spilly 7:ebc76b0f21da 224 temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
Spilly 5:40ac894e0fa7 225
Spilly 7:ebc76b0f21da 226 temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
Spilly 7:ebc76b0f21da 227 temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
Spilly 7:ebc76b0f21da 228 temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
Spilly 7:ebc76b0f21da 229
Spilly 7:ebc76b0f21da 230 // Since we compensated for pitch, x-z-plane-component equals z-component:
Spilly 7:ebc76b0f21da 231 roll = atan2(temp2[1], temp2[2]);
Spilly 5:40ac894e0fa7 232 }
Spilly 5:40ac894e0fa7 233
Spilly 5:40ac894e0fa7 234 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 235 // Filtered_Attitude()
Spilly 5:40ac894e0fa7 236 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 237
Spilly 5:40ac894e0fa7 238 void Filtered_Attitude()
Spilly 5:40ac894e0fa7 239 {
Spilly 7:ebc76b0f21da 240 float temp1[3];
Spilly 7:ebc76b0f21da 241 float temp2[3];
Spilly 7:ebc76b0f21da 242 float xAxis[3] = {1.0f, 0.0f, 0.0f};
Spilly 7:ebc76b0f21da 243
Spilly 7:ebc76b0f21da 244 // GET PITCH
Spilly 7:ebc76b0f21da 245 // Using y-z-plane-component/x-component of gravity vector
Spilly 7:ebc76b0f21da 246 pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
Spilly 7:ebc76b0f21da 247
Spilly 7:ebc76b0f21da 248 //printf("P = %f", pitch);
Spilly 7:ebc76b0f21da 249
Spilly 7:ebc76b0f21da 250 // GET ROLL
Spilly 7:ebc76b0f21da 251 // Compensate pitch of gravity vector
Spilly 7:ebc76b0f21da 252 temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
Spilly 7:ebc76b0f21da 253 temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
Spilly 7:ebc76b0f21da 254 temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
Spilly 7:ebc76b0f21da 255
Spilly 7:ebc76b0f21da 256 temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
Spilly 7:ebc76b0f21da 257 temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
Spilly 7:ebc76b0f21da 258 temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
Spilly 7:ebc76b0f21da 259
Spilly 7:ebc76b0f21da 260 // Since we compensated for pitch, x-z-plane-component equals z-component:
Spilly 7:ebc76b0f21da 261 roll = atan2(temp2[1], temp2[2]);
Spilly 0:e79311aae7ed 262 }
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 5:40ac894e0fa7 270 //int updateAngles()
Spilly 0:e79311aae7ed 271 {
Spilly 5:40ac894e0fa7 272 Filtered_Attitude();
Spilly 5:40ac894e0fa7 273 Compass_Heading();
Spilly 5:40ac894e0fa7 274 }
Spilly 5:40ac894e0fa7 275
Spilly 5:40ac894e0fa7 276 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 277 // normalizeGravity
Spilly 5:40ac894e0fa7 278 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 279
Spilly 5:40ac894e0fa7 280 void normalizeGravity()
Spilly 5:40ac894e0fa7 281 {
Spilly 5:40ac894e0fa7 282 float accumulator = 0;
Spilly 5:40ac894e0fa7 283 int sampleCount = 0;
Spilly 5:40ac894e0fa7 284
Spilly 5:40ac894e0fa7 285
Spilly 5:40ac894e0fa7 286 //We'll take a certain number of samples and then average them to calculate the average
Spilly 5:40ac894e0fa7 287 while (sampleCount < AVG_COUNT)
Spilly 5:40ac894e0fa7 288 {
Spilly 5:40ac894e0fa7 289 getAccel();
Spilly 5:40ac894e0fa7 290 //add current sample to previous samples
Spilly 5:40ac894e0fa7 291 accumulator += sqrt((accel[0] * accel[0]) + (accel[1] * accel[1]) + (accel[2] * accel[2]));
Spilly 5:40ac894e0fa7 292 sampleCount++;
Spilly 5:40ac894e0fa7 293 //Make sure the IMU has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 294 wait(0.06);
Spilly 5:40ac894e0fa7 295 }
Spilly 5:40ac894e0fa7 296
Spilly 5:40ac894e0fa7 297
Spilly 5:40ac894e0fa7 298 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 299 normalizedGravity = accumulator / AVG_COUNT;
Spilly 5:40ac894e0fa7 300
Spilly 5:40ac894e0fa7 301 }
Spilly 5:40ac894e0fa7 302 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 303 // gyroAvg
Spilly 5:40ac894e0fa7 304 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 305
Spilly 5:40ac894e0fa7 306 void gyroAvg()
Spilly 2:503a5ac6c3b6 307 {
Spilly 5:40ac894e0fa7 308 float accumulator[3] = {0,0,0};
Spilly 5:40ac894e0fa7 309 int sampleCount = 0;
Spilly 5:40ac894e0fa7 310
Spilly 5:40ac894e0fa7 311
Spilly 5:40ac894e0fa7 312 //We'll take a certain number of samples and then average them to calculate the offset
Spilly 5:40ac894e0fa7 313 while (sampleCount < AVG_COUNT)
Spilly 5:40ac894e0fa7 314 {
Spilly 5:40ac894e0fa7 315 //get gyro data
Spilly 5:40ac894e0fa7 316 gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
Spilly 5:40ac894e0fa7 317 //add current sample to previous samples
Spilly 5:40ac894e0fa7 318 accumulator[2] += degGyro[2];
Spilly 5:40ac894e0fa7 319 sampleCount++;
Spilly 5:40ac894e0fa7 320 //Make sure the gyro has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 321 wait(GYRO_SAMPLE_PERIOD);
Spilly 5:40ac894e0fa7 322 }
Spilly 5:40ac894e0fa7 323
Spilly 5:40ac894e0fa7 324
Spilly 5:40ac894e0fa7 325 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 326 degGyro[2] = accumulator[2] / AVG_COUNT;
Spilly 5:40ac894e0fa7 327
Spilly 5:40ac894e0fa7 328 }
Spilly 5:40ac894e0fa7 329
Spilly 5:40ac894e0fa7 330 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 331 // gyroCal
Spilly 5:40ac894e0fa7 332 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 333
Spilly 5:40ac894e0fa7 334 void gyroCal()
Spilly 5:40ac894e0fa7 335 {
Spilly 5:40ac894e0fa7 336 float accumulator[3] = {0,0,0};
Spilly 5:40ac894e0fa7 337 int sampleCount = 0;
Spilly 5:40ac894e0fa7 338
Spilly 5:40ac894e0fa7 339
Spilly 5:40ac894e0fa7 340 //We'll take a certain number of samples and then average them to calculate the offset
Spilly 5:40ac894e0fa7 341 while (sampleCount < CALIBRATION_COUNT)
Spilly 5:40ac894e0fa7 342 {
Spilly 5:40ac894e0fa7 343 float gyroData[3];
Spilly 5:40ac894e0fa7 344 //Make sure the gyro has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 345 wait(GYRO_SAMPLE_PERIOD);
Spilly 5:40ac894e0fa7 346 //get gyro data
Spilly 5:40ac894e0fa7 347 gyro.read(&gyroData[0],&gyroData[1],&gyroData[2]);
Spilly 5:40ac894e0fa7 348 for(int i = 0; i < 3; i++)
Spilly 5:40ac894e0fa7 349 {
Spilly 5:40ac894e0fa7 350 //add current sample to previous samples
Spilly 5:40ac894e0fa7 351 accumulator[i] += gyroData[i];
Spilly 5:40ac894e0fa7 352 }
Spilly 5:40ac894e0fa7 353 sampleCount++;
Spilly 5:40ac894e0fa7 354 }
Spilly 5:40ac894e0fa7 355
Spilly 5:40ac894e0fa7 356 for(int i = 0; i < 3; i++)
Spilly 5:40ac894e0fa7 357 {
Spilly 5:40ac894e0fa7 358 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 359 gyroOffset[i] = accumulator[i] / CALIBRATION_COUNT;
Spilly 5:40ac894e0fa7 360 }
Spilly 5:40ac894e0fa7 361 }
Spilly 5:40ac894e0fa7 362
Spilly 7:ebc76b0f21da 363 /********************************************************************************************************************************/
Spilly 7:ebc76b0f21da 364 // Only Get Gyro Data
Spilly 7:ebc76b0f21da 365 /********************************************************************************************************************************/
Spilly 7:ebc76b0f21da 366
Spilly 5:40ac894e0fa7 367 void gyroOnly()
Spilly 5:40ac894e0fa7 368 {
Spilly 5:40ac894e0fa7 369 gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
Spilly 5:40ac894e0fa7 370 //compensate for gyro offset
Spilly 5:40ac894e0fa7 371 for(int i=0;i<=3;i++)
Spilly 5:40ac894e0fa7 372 {
Spilly 5:40ac894e0fa7 373 degGyro[i] -= gyroOffset[i];
Spilly 5:40ac894e0fa7 374 }
Spilly 7:ebc76b0f21da 375 }