David Spillman / Mbed 2 deprecated GPSNavigationNew

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

modSensData.h@5:40ac894e0fa7, 2015-04-05 (annotated)

Committer:
Spilly
Date:
Sun Apr 05 01:45:29 2015 +0000
Revision:
5:40ac894e0fa7
Parent:
4:a397b44a0fe8
Child:
7:ebc76b0f21da
Updated commands

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 5:40ac894e0fa7 22 //Variable for low pass filter
Spilly 5:40ac894e0fa7 23 #define ALPHA_H 0.2f
Spilly 5:40ac894e0fa7 24 #define ALPHA_A 0.1f
Spilly 5:40ac894e0fa7 25 #define MAX_DELTA_HEADING 20
Spilly 5:40ac894e0fa7 26
Spilly 5:40ac894e0fa7 27 #define M_PI 3.1415926535897932384626433832795f
Spilly 5:40ac894e0fa7 28 #define TWO_PI 6.283185307179586476925286766559f
Spilly 5:40ac894e0fa7 29 #define RADTODEGREE 57.295779513082320876798154814105f
Spilly 5:40ac894e0fa7 30 #define DEGREETORAD 0.01745329251994329576923690768489f
Spilly 5:40ac894e0fa7 31 #define GRAVITY 1.0f
Spilly 5:40ac894e0fa7 32 #define THRESHOLD 0.02f
Spilly 5:40ac894e0fa7 33
Spilly 5:40ac894e0fa7 34 //gyro sample period in seconds
Spilly 5:40ac894e0fa7 35 #define GYRO_SAMPLE_PERIOD 0.01f
Spilly 5:40ac894e0fa7 36 #define CALIBRATION_COUNT 64
Spilly 5:40ac894e0fa7 37 #define AVG_COUNT 10
Spilly 0:e79311aae7ed 38
Spilly 4:a397b44a0fe8 39 // "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
Spilly 5:40ac894e0fa7 40 #define ACCEL_X_MIN -1.000000000f
Spilly 5:40ac894e0fa7 41 #define ACCEL_X_MAX 1.023437500f
Spilly 5:40ac894e0fa7 42 #define ACCEL_Y_MIN -1.015625000f
Spilly 5:40ac894e0fa7 43 #define ACCEL_Y_MAX 1.023437500f
Spilly 5:40ac894e0fa7 44 #define ACCEL_Z_MIN -1.023437500f
Spilly 5:40ac894e0fa7 45 #define ACCEL_Z_MAX 0.960937500f
Spilly 0:e79311aae7ed 46
Spilly 4:a397b44a0fe8 47 // "magn x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
Spilly 5:40ac894e0fa7 48 #define MAGN_X_MIN (-0.370909f)
Spilly 5:40ac894e0fa7 49 #define MAGN_X_MAX (0.569091f)
Spilly 5:40ac894e0fa7 50 #define MAGN_Y_MIN (-0.516364f)
Spilly 5:40ac894e0fa7 51 #define MAGN_Y_MAX (0.392727f)
Spilly 5:40ac894e0fa7 52 #define MAGN_Z_MIN (-0.618367f)
Spilly 5:40ac894e0fa7 53 #define MAGN_Z_MAX (0.408163f)
Spilly 5:40ac894e0fa7 54
Spilly 5:40ac894e0fa7 55
Spilly 5:40ac894e0fa7 56 //Period in seconds of heading PID loop
Spilly 5:40ac894e0fa7 57 #define COMPASS_PERIOD 0.060f
Spilly 5:40ac894e0fa7 58
Spilly 5:40ac894e0fa7 59 #define YAW_VAR_TOLER 5.0f
Spilly 0:e79311aae7ed 60
Spilly 4:a397b44a0fe8 61 // Sensor calibration scale and offset values
Spilly 4:a397b44a0fe8 62 #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 63 #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 64 #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 65 #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
Spilly 4:a397b44a0fe8 66 #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
Spilly 4:a397b44a0fe8 67 #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
Spilly 4:a397b44a0fe8 68
Spilly 4:a397b44a0fe8 69 #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 70 #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 71 #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
Spilly 5:40ac894e0fa7 72 //not sure what the "normal" value is, thus not using scale for magnetometer
Spilly 5:40ac894e0fa7 73 //#define MAGN_X_SCALE (??? / (MAGN_X_MAX - MAGN_X_OFFSET))
Spilly 5:40ac894e0fa7 74 //#define MAGN_Y_SCALE (??? / (MAGN_Y_MAX - MAGN_Y_OFFSET))
Spilly 5:40ac894e0fa7 75 //#define MAGN_Z_SCALE (??? / (MAGN_Z_MAX - MAGN_Z_OFFSET))
Spilly 4:a397b44a0fe8 76
Spilly 4:a397b44a0fe8 77
Spilly 5:40ac894e0fa7 78 float magnetom[3] = {0,0,0};
Spilly 5:40ac894e0fa7 79 float accel[3] = {0,0,0};
Spilly 5:40ac894e0fa7 80 float degGyro[3] = {0,0,0};
Spilly 5:40ac894e0fa7 81 float prevDegGyro[3]= {0,0,0};
Spilly 5:40ac894e0fa7 82 float gyroOffset[3] = {0,0,0};
Spilly 5:40ac894e0fa7 83 float roll = 0, pitch = 0, yaw = 0;
Spilly 5:40ac894e0fa7 84 float totalAccel = 0;
Spilly 5:40ac894e0fa7 85 float normalizedGravity;
Spilly 0:e79311aae7ed 86
Spilly 5:40ac894e0fa7 87 float filteredYaw, filteredPitch, filteredRoll;
Spilly 0:e79311aae7ed 88
Spilly 5:40ac894e0fa7 89 float lowPassAccel[3] = {0.0000001f,0.0000001f,0.0000001f};
Spilly 5:40ac894e0fa7 90 float lowPassMagnetom[3] = {0.0000001f,0.0000001f,0.0000001f};
Spilly 0:e79311aae7ed 91
Spilly 5:40ac894e0fa7 92 float prevYaw = 0;
Spilly 5:40ac894e0fa7 93 int cycleCount = 0;
Spilly 5:40ac894e0fa7 94 int firstPass = 1;
Spilly 5:40ac894e0fa7 95 /*
Spilly 5:40ac894e0fa7 96 float lowPass(float input, float output)
Spilly 5:40ac894e0fa7 97 {
Spilly 5:40ac894e0fa7 98 if (output == 0.0000001f) return input;
Spilly 5:40ac894e0fa7 99
Spilly 5:40ac894e0fa7 100 output = (ALPHA * input) + ((1 - ALPHA) * output);
Spilly 5:40ac894e0fa7 101
Spilly 5:40ac894e0fa7 102 return output;
Spilly 5:40ac894e0fa7 103 }
Spilly 5:40ac894e0fa7 104 */
Spilly 5:40ac894e0fa7 105 float lowPass(float input, float output, int select)
Spilly 5:40ac894e0fa7 106 {
Spilly 5:40ac894e0fa7 107 //Accelerometer alpha
Spilly 5:40ac894e0fa7 108 if(select == 1)
Spilly 5:40ac894e0fa7 109 {
Spilly 5:40ac894e0fa7 110 if (output == 0.0000001f) return input;
Spilly 5:40ac894e0fa7 111
Spilly 5:40ac894e0fa7 112 output = output + ALPHA_A * (input - output);
Spilly 5:40ac894e0fa7 113
Spilly 5:40ac894e0fa7 114 return output;
Spilly 5:40ac894e0fa7 115 }
Spilly 5:40ac894e0fa7 116 //Heading alpha
Spilly 5:40ac894e0fa7 117 else
Spilly 5:40ac894e0fa7 118 {
Spilly 5:40ac894e0fa7 119 if (output == 0.0000001f) return input;
Spilly 5:40ac894e0fa7 120
Spilly 5:40ac894e0fa7 121 output = output + ALPHA_H * (input - output);
Spilly 5:40ac894e0fa7 122
Spilly 5:40ac894e0fa7 123 return output;
Spilly 5:40ac894e0fa7 124 }
Spilly 5:40ac894e0fa7 125 }
Spilly 0:e79311aae7ed 126
Spilly 0:e79311aae7ed 127 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 128 // getMagn
Spilly 5:40ac894e0fa7 129 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 130
Spilly 5:40ac894e0fa7 131 void getMagn()
Spilly 5:40ac894e0fa7 132 {
Spilly 5:40ac894e0fa7 133 compass.ReadMagnOnly(&magnetom[0], &magnetom[1], &magnetom[2]);
Spilly 5:40ac894e0fa7 134
Spilly 5:40ac894e0fa7 135 //Compensate magnetometer error
Spilly 5:40ac894e0fa7 136 magnetom[0] -= MAGN_X_OFFSET;
Spilly 5:40ac894e0fa7 137 magnetom[1] -= MAGN_Y_OFFSET;
Spilly 5:40ac894e0fa7 138 magnetom[2] -= MAGN_Z_OFFSET;
Spilly 5:40ac894e0fa7 139 }
Spilly 5:40ac894e0fa7 140 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 141 // getAccel
Spilly 0:e79311aae7ed 142 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 143
Spilly 5:40ac894e0fa7 144 void getAccel()
Spilly 0:e79311aae7ed 145 {
Spilly 5:40ac894e0fa7 146 compass.ReadAccOnly(&accel[0], &accel[1], &accel[2]);
Spilly 4:a397b44a0fe8 147
Spilly 4:a397b44a0fe8 148 //Compensate accelerometer error
Spilly 5:40ac894e0fa7 149 accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
Spilly 5:40ac894e0fa7 150 accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
Spilly 5:40ac894e0fa7 151 accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
Spilly 5:40ac894e0fa7 152
Spilly 4:a397b44a0fe8 153
Spilly 5:40ac894e0fa7 154 for(int i = 0; i <= 3; i++)
Spilly 5:40ac894e0fa7 155 {
Spilly 5:40ac894e0fa7 156 lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
Spilly 5:40ac894e0fa7 157 }
Spilly 0:e79311aae7ed 158 }
Spilly 5:40ac894e0fa7 159 void gyroIntegral()
Spilly 5:40ac894e0fa7 160 {
Spilly 5:40ac894e0fa7 161 float yawDiff = yaw - prevYaw;
Spilly 5:40ac894e0fa7 162 float absYawDiff = sqrt(yawDiff * yawDiff);
Spilly 5:40ac894e0fa7 163 //vC[x] = vP[x] + ((aP[x] + ((inertialAccel[x] - aP[x]) / 2)) * deltaT);
Spilly 5:40ac894e0fa7 164 float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * COMPASS_PERIOD);
Spilly 5:40ac894e0fa7 165 float absGyroInt = sqrt(gyroInt * gyroInt);
Spilly 5:40ac894e0fa7 166 prevDegGyro[2] = degGyro[2];
Spilly 5:40ac894e0fa7 167 if(absYawDiff > absGyroInt)
Spilly 5:40ac894e0fa7 168 {
Spilly 5:40ac894e0fa7 169 yaw = prevYaw + gyroInt;
Spilly 5:40ac894e0fa7 170 }
Spilly 5:40ac894e0fa7 171 }
Spilly 0:e79311aae7ed 172 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 173 // Compass Heading
Spilly 0:e79311aae7ed 174 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 175
Spilly 0:e79311aae7ed 176 void Compass_Heading()
Spilly 0:e79311aae7ed 177 {
Spilly 5:40ac894e0fa7 178
Spilly 5:40ac894e0fa7 179 float mag_x;
Spilly 5:40ac894e0fa7 180 float mag_y;
Spilly 5:40ac894e0fa7 181 //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
Spilly 5:40ac894e0fa7 182 //{
Spilly 5:40ac894e0fa7 183 float cos_roll;
Spilly 5:40ac894e0fa7 184 float sin_roll;
Spilly 5:40ac894e0fa7 185 float cos_pitch;
Spilly 5:40ac894e0fa7 186 float sin_pitch;
Spilly 5:40ac894e0fa7 187
Spilly 5:40ac894e0fa7 188 //saves a few processor cycles by calculating and storing values
Spilly 5:40ac894e0fa7 189 cos_roll = cos(roll);
Spilly 5:40ac894e0fa7 190 sin_roll = sin(roll);
Spilly 5:40ac894e0fa7 191 cos_pitch = cos(pitch);
Spilly 5:40ac894e0fa7 192 sin_pitch = sin(pitch);
Spilly 5:40ac894e0fa7 193
Spilly 5:40ac894e0fa7 194 // Tilt compensated magnetic field X
Spilly 5:40ac894e0fa7 195 //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
Spilly 5:40ac894e0fa7 196 mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
Spilly 5:40ac894e0fa7 197 // Tilt compensated magnetic field Y
Spilly 5:40ac894e0fa7 198 //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
Spilly 5:40ac894e0fa7 199 mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
Spilly 5:40ac894e0fa7 200 //}
Spilly 5:40ac894e0fa7 201 /*
Spilly 5:40ac894e0fa7 202 else
Spilly 5:40ac894e0fa7 203 {
Spilly 5:40ac894e0fa7 204 mag_x = magnetom[0] ;
Spilly 5:40ac894e0fa7 205 mag_y = magnetom[1];
Spilly 5:40ac894e0fa7 206 }
Spilly 5:40ac894e0fa7 207 */
Spilly 5:40ac894e0fa7 208 // Magnetic Heading
Spilly 5:40ac894e0fa7 209 yaw = atan2(-mag_x, mag_y);
Spilly 5:40ac894e0fa7 210
Spilly 5:40ac894e0fa7 211
Spilly 5:40ac894e0fa7 212 //make negative angles positive
Spilly 5:40ac894e0fa7 213 if(yaw < 0) yaw = yaw + TWO_PI;
Spilly 5:40ac894e0fa7 214 //yaw = yaw + M_PI;
Spilly 5:40ac894e0fa7 215 yaw = yaw * RADTODEGREE;
Spilly 0:e79311aae7ed 216 }
Spilly 0:e79311aae7ed 217
Spilly 0:e79311aae7ed 218 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 219 // getAttitude
Spilly 0:e79311aae7ed 220 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 221
Spilly 0:e79311aae7ed 222 void getAttitude()
Spilly 0:e79311aae7ed 223 {
Spilly 0:e79311aae7ed 224 float temp1[3];
Spilly 0:e79311aae7ed 225 float temp2[3];
Spilly 0:e79311aae7ed 226 float xAxis[3] = {1.0f, 0.0f, 0.0f};
Spilly 0:e79311aae7ed 227
Spilly 0:e79311aae7ed 228 // GET PITCH
Spilly 0:e79311aae7ed 229 // Using y-z-plane-component/x-component of gravity vector
Spilly 0:e79311aae7ed 230 pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
Spilly 0:e79311aae7ed 231
Spilly 0:e79311aae7ed 232 //printf("P = %f", pitch);
Spilly 0:e79311aae7ed 233
Spilly 0:e79311aae7ed 234 // GET ROLL
Spilly 0:e79311aae7ed 235 // Compensate pitch of gravity vector
Spilly 0:e79311aae7ed 236 temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
Spilly 0:e79311aae7ed 237 temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
Spilly 0:e79311aae7ed 238 temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
Spilly 0:e79311aae7ed 239
Spilly 0:e79311aae7ed 240 temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
Spilly 0:e79311aae7ed 241 temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
Spilly 0:e79311aae7ed 242 temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
Spilly 0:e79311aae7ed 243
Spilly 0:e79311aae7ed 244 // Since we compensated for pitch, x-z-plane-component equals z-component:
Spilly 0:e79311aae7ed 245 roll = atan2(temp2[1], temp2[2]);
Spilly 0:e79311aae7ed 246 }
Spilly 0:e79311aae7ed 247
Spilly 0:e79311aae7ed 248 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 249 // Compass Heading
Spilly 0:e79311aae7ed 250 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 251
Spilly 5:40ac894e0fa7 252 void Filtered_Compass_Heading()
Spilly 0:e79311aae7ed 253 {
Spilly 5:40ac894e0fa7 254
Spilly 5:40ac894e0fa7 255 float mag_x;
Spilly 5:40ac894e0fa7 256 float mag_y;
Spilly 5:40ac894e0fa7 257 //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
Spilly 5:40ac894e0fa7 258 //{
Spilly 5:40ac894e0fa7 259 float cos_roll;
Spilly 5:40ac894e0fa7 260 float sin_roll;
Spilly 5:40ac894e0fa7 261 float cos_pitch;
Spilly 5:40ac894e0fa7 262 float sin_pitch;
Spilly 5:40ac894e0fa7 263
Spilly 5:40ac894e0fa7 264 //saves a few processor cycles by calculating and storing values
Spilly 5:40ac894e0fa7 265 cos_roll = cos(filteredRoll);
Spilly 5:40ac894e0fa7 266 sin_roll = sin(filteredRoll);
Spilly 5:40ac894e0fa7 267 cos_pitch = cos(filteredPitch);
Spilly 5:40ac894e0fa7 268 sin_pitch = sin(filteredPitch);
Spilly 5:40ac894e0fa7 269
Spilly 5:40ac894e0fa7 270 // Tilt compensated magnetic field X
Spilly 5:40ac894e0fa7 271 //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
Spilly 5:40ac894e0fa7 272 mag_x = lowPassMagnetom[0] * cos_pitch + lowPassMagnetom[1] * sin_roll * sin_pitch + lowPassMagnetom[2] * cos_roll * sin_pitch;
Spilly 5:40ac894e0fa7 273 // Tilt compensated magnetic field Y
Spilly 5:40ac894e0fa7 274 //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
Spilly 5:40ac894e0fa7 275 mag_y = lowPassMagnetom[1] * cos_roll - lowPassMagnetom[2] * sin_roll;
Spilly 5:40ac894e0fa7 276 //}
Spilly 5:40ac894e0fa7 277 /*
Spilly 5:40ac894e0fa7 278 else
Spilly 5:40ac894e0fa7 279 {
Spilly 5:40ac894e0fa7 280 mag_x = magnetom[0] ;
Spilly 5:40ac894e0fa7 281 mag_y = magnetom[1];
Spilly 5:40ac894e0fa7 282 }
Spilly 5:40ac894e0fa7 283 */
Spilly 5:40ac894e0fa7 284 // Magnetic Heading
Spilly 5:40ac894e0fa7 285 filteredYaw = atan2(-mag_x, mag_y);
Spilly 5:40ac894e0fa7 286
Spilly 5:40ac894e0fa7 287
Spilly 5:40ac894e0fa7 288 //make negative angles positive
Spilly 5:40ac894e0fa7 289 if(filteredYaw < 0) filteredYaw = filteredYaw + TWO_PI;
Spilly 5:40ac894e0fa7 290 //yaw = yaw + M_PI;
Spilly 5:40ac894e0fa7 291 filteredYaw = filteredYaw * RADTODEGREE;
Spilly 5:40ac894e0fa7 292 }
Spilly 5:40ac894e0fa7 293
Spilly 5:40ac894e0fa7 294 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 295 // Filtered_Attitude()
Spilly 5:40ac894e0fa7 296 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 297
Spilly 5:40ac894e0fa7 298 void Filtered_Attitude()
Spilly 5:40ac894e0fa7 299 {
Spilly 5:40ac894e0fa7 300 float temp1[3];
Spilly 5:40ac894e0fa7 301 float temp2[3];
Spilly 5:40ac894e0fa7 302 float xAxis[3] = {1.0f, 0.0f, 0.0f};
Spilly 0:e79311aae7ed 303
Spilly 5:40ac894e0fa7 304 // GET PITCH
Spilly 5:40ac894e0fa7 305 // Using y-z-plane-component/x-component of gravity vector
Spilly 5:40ac894e0fa7 306 pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
Spilly 0:e79311aae7ed 307
Spilly 5:40ac894e0fa7 308 //printf("P = %f", pitch);
Spilly 0:e79311aae7ed 309
Spilly 5:40ac894e0fa7 310 // GET ROLL
Spilly 5:40ac894e0fa7 311 // Compensate pitch of gravity vector
Spilly 5:40ac894e0fa7 312 temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
Spilly 5:40ac894e0fa7 313 temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
Spilly 5:40ac894e0fa7 314 temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
Spilly 0:e79311aae7ed 315
Spilly 5:40ac894e0fa7 316 temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
Spilly 5:40ac894e0fa7 317 temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
Spilly 5:40ac894e0fa7 318 temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
Spilly 5:40ac894e0fa7 319
Spilly 5:40ac894e0fa7 320 // Since we compensated for pitch, x-z-plane-component equals z-component:
Spilly 5:40ac894e0fa7 321 roll = atan2(temp2[1], temp2[2]);
Spilly 0:e79311aae7ed 322 }
Spilly 0:e79311aae7ed 323
Spilly 0:e79311aae7ed 324
Spilly 5:40ac894e0fa7 325 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 326 // updateAngles
Spilly 5:40ac894e0fa7 327 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 328
Spilly 5:40ac894e0fa7 329 void updateAngles()
Spilly 5:40ac894e0fa7 330 //int updateAngles()
Spilly 0:e79311aae7ed 331 {
Spilly 5:40ac894e0fa7 332 //readIMU();
Spilly 5:40ac894e0fa7 333 //getAttitude();
Spilly 5:40ac894e0fa7 334 Filtered_Attitude();
Spilly 5:40ac894e0fa7 335 Compass_Heading();
Spilly 5:40ac894e0fa7 336 //Filtered_Attitude();
Spilly 5:40ac894e0fa7 337 //Filtered_Compass_Heading();
Spilly 5:40ac894e0fa7 338 /*
Spilly 5:40ac894e0fa7 339 totalAccel = sqrt((lowPassAccel[0] * lowPassAccel[0]) + (lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2]));
Spilly 5:40ac894e0fa7 340
Spilly 5:40ac894e0fa7 341 prevYaw = yaw;
Spilly 5:40ac894e0fa7 342 getAttitude();
Spilly 5:40ac894e0fa7 343 Compass_Heading();
Spilly 5:40ac894e0fa7 344
Spilly 5:40ac894e0fa7 345 if((sqrt((prevYaw - yaw) * (prevYaw - yaw)) >= YAW_VAR_TOLER) && (firstPass != 1))
Spilly 5:40ac894e0fa7 346 {
Spilly 5:40ac894e0fa7 347 yaw = prevYaw;
Spilly 5:40ac894e0fa7 348 }
Spilly 5:40ac894e0fa7 349 else if(firstPass == 1)
Spilly 5:40ac894e0fa7 350 {
Spilly 5:40ac894e0fa7 351 firstPass = 0;
Spilly 5:40ac894e0fa7 352 }
Spilly 5:40ac894e0fa7 353
Spilly 5:40ac894e0fa7 354 //if((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * accel[2]) <= THRESHOLD))
Spilly 5:40ac894e0fa7 355 if((totalAccel <= (normalizedGravity + THRESHOLD)) && (totalAccel >= (normalizedGravity - THRESHOLD)))
Spilly 2:503a5ac6c3b6 356 {
Spilly 2:503a5ac6c3b6 357 //float prevYaw = yaw;
Spilly 2:503a5ac6c3b6 358 getAttitude();
Spilly 2:503a5ac6c3b6 359 Compass_Heading();
Spilly 2:503a5ac6c3b6 360 //float absOfYawDiff = sqrt((prevYaw - yaw) * (prevYaw - yaw));
Spilly 2:503a5ac6c3b6 361 //check for noise
Spilly 2:503a5ac6c3b6 362 //if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
Spilly 2:503a5ac6c3b6 363 //{
Spilly 5:40ac894e0fa7 364 return 1;
Spilly 2:503a5ac6c3b6 365 //}
Spilly 2:503a5ac6c3b6 366 //return 0;
Spilly 2:503a5ac6c3b6 367
Spilly 2:503a5ac6c3b6 368 }
Spilly 5:40ac894e0fa7 369 */
Spilly 5:40ac894e0fa7 370 //return 0;
Spilly 5:40ac894e0fa7 371 //else Compass_Heading();
Spilly 5:40ac894e0fa7 372 //else return 0;
Spilly 5:40ac894e0fa7 373 //getAttitude();
Spilly 5:40ac894e0fa7 374 //Compass_Heading();
Spilly 5:40ac894e0fa7 375 //gyroIntegral();
Spilly 5:40ac894e0fa7 376 }
Spilly 5:40ac894e0fa7 377
Spilly 5:40ac894e0fa7 378 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 379 // magAvg
Spilly 5:40ac894e0fa7 380 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 381
Spilly 5:40ac894e0fa7 382 void compassAvg()
Spilly 5:40ac894e0fa7 383 {
Spilly 5:40ac894e0fa7 384 float accumulator = 0;
Spilly 5:40ac894e0fa7 385 //float prevYaw[AVG_COUNT];
Spilly 5:40ac894e0fa7 386 int sampleCount = 0;
Spilly 5:40ac894e0fa7 387
Spilly 5:40ac894e0fa7 388
Spilly 5:40ac894e0fa7 389 //We'll take a certain number of samples and then average them to calculate the offset
Spilly 5:40ac894e0fa7 390 while (sampleCount < AVG_COUNT)
Spilly 5:40ac894e0fa7 391 {
Spilly 5:40ac894e0fa7 392 updateAngles();
Spilly 5:40ac894e0fa7 393
Spilly 5:40ac894e0fa7 394 //add current sample to previous samples
Spilly 5:40ac894e0fa7 395 accumulator += yaw;
Spilly 5:40ac894e0fa7 396
Spilly 5:40ac894e0fa7 397 sampleCount++;
Spilly 5:40ac894e0fa7 398 //Make sure the gyro has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 399 wait(COMPASS_PERIOD);
Spilly 5:40ac894e0fa7 400 }
Spilly 5:40ac894e0fa7 401
Spilly 5:40ac894e0fa7 402 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 403 yaw = accumulator / AVG_COUNT;
Spilly 5:40ac894e0fa7 404
Spilly 5:40ac894e0fa7 405
Spilly 5:40ac894e0fa7 406 }
Spilly 5:40ac894e0fa7 407
Spilly 5:40ac894e0fa7 408 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 409 // magLowPass
Spilly 5:40ac894e0fa7 410 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 411 void compLowPass()
Spilly 5:40ac894e0fa7 412 {
Spilly 5:40ac894e0fa7 413 /*
Spilly 5:40ac894e0fa7 414 prevMagnetom[5][3];
Spilly 5:40ac894e0fa7 415 //read sensors five times
Spilly 5:40ac894e0fa7 416 for(int x = 0; x<= 5; i++)
Spilly 5:40ac894e0fa7 417 {
Spilly 5:40ac894e0fa7 418 for(int i = 0; i<=3; i++)
Spilly 5:40ac894e0fa7 419 {
Spilly 5:40ac894e0fa7 420 prevMagnetom[x, i] = magnetom[i];
Spilly 5:40ac894e0fa7 421 }
Spilly 5:40ac894e0fa7 422 readIMU();
Spilly 5:40ac894e0fa7 423 }
Spilly 5:40ac894e0fa7 424 float diff[5][3];
Spilly 5:40ac894e0fa7 425 for(int x = 1; x<= 5; i++)
Spilly 5:40ac894e0fa7 426 {
Spilly 5:40ac894e0fa7 427 sqrt(prevMagnetom[x][0]
Spilly 5:40ac894e0fa7 428 for(int i = 0; i<=3; i++)
Spilly 5:40ac894e0fa7 429 {
Spilly 5:40ac894e0fa7 430 if(diff[x][i] = sqrt((prevMagnetom[x][i] - prevMagnemtom[x - 1]) * (prevMagnetom[x][i] - prevMagnemtom[x - 1])) >= MAG_VAR_TOLER)
Spilly 5:40ac894e0fa7 431 {
Spilly 5:40ac894e0fa7 432
Spilly 5:40ac894e0fa7 433 }
Spilly 5:40ac894e0fa7 434 }
Spilly 5:40ac894e0fa7 435 }
Spilly 5:40ac894e0fa7 436
Spilly 5:40ac894e0fa7 437 float deltaHeading = 0;
Spilly 5:40ac894e0fa7 438 updateAngles();
Spilly 5:40ac894e0fa7 439 deltaHeading = sqrt((yaw - prevYaw[cycleCount]) * (yaw - prevYaw[cycleCount]));
Spilly 5:40ac894e0fa7 440
Spilly 5:40ac894e0fa7 441 if(deltaHeading >= MAX_DELTA_HEADING)
Spilly 5:40ac894e0fa7 442 {
Spilly 5:40ac894e0fa7 443 yaw = (yaw + prevYaw[cycleCount]) / 2.0f;
Spilly 5:40ac894e0fa7 444 }
Spilly 5:40ac894e0fa7 445 cycleCount = cycleCount + 1;
Spilly 5:40ac894e0fa7 446 prevYaw[cycleCount] = yaw;
Spilly 5:40ac894e0fa7 447
Spilly 5:40ac894e0fa7 448 if(cycleCount >= 4)
Spilly 5:40ac894e0fa7 449 {
Spilly 5:40ac894e0fa7 450 cycleCount = 0;
Spilly 5:40ac894e0fa7 451 prevYaw = {0,0,0,0,0}
Spilly 5:40ac894e0fa7 452 }
Spilly 5:40ac894e0fa7 453 */
Spilly 2:503a5ac6c3b6 454 }
Spilly 2:503a5ac6c3b6 455
Spilly 5:40ac894e0fa7 456
Spilly 5:40ac894e0fa7 457 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 458 // normalizeGravity
Spilly 5:40ac894e0fa7 459 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 460
Spilly 5:40ac894e0fa7 461 void normalizeGravity()
Spilly 5:40ac894e0fa7 462 {
Spilly 5:40ac894e0fa7 463 float accumulator = 0;
Spilly 5:40ac894e0fa7 464 int sampleCount = 0;
Spilly 5:40ac894e0fa7 465
Spilly 5:40ac894e0fa7 466
Spilly 5:40ac894e0fa7 467 //We'll take a certain number of samples and then average them to calculate the average
Spilly 5:40ac894e0fa7 468 while (sampleCount < AVG_COUNT)
Spilly 5:40ac894e0fa7 469 {
Spilly 5:40ac894e0fa7 470 getAccel();
Spilly 5:40ac894e0fa7 471 //add current sample to previous samples
Spilly 5:40ac894e0fa7 472 accumulator += sqrt((accel[0] * accel[0]) + (accel[1] * accel[1]) + (accel[2] * accel[2]));
Spilly 5:40ac894e0fa7 473 sampleCount++;
Spilly 5:40ac894e0fa7 474 //Make sure the IMU has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 475 wait(0.06);
Spilly 5:40ac894e0fa7 476 }
Spilly 5:40ac894e0fa7 477
Spilly 5:40ac894e0fa7 478
Spilly 5:40ac894e0fa7 479 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 480 normalizedGravity = accumulator / AVG_COUNT;
Spilly 5:40ac894e0fa7 481
Spilly 5:40ac894e0fa7 482 }
Spilly 5:40ac894e0fa7 483 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 484 // gyroAvg
Spilly 5:40ac894e0fa7 485 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 486
Spilly 5:40ac894e0fa7 487 void gyroAvg()
Spilly 2:503a5ac6c3b6 488 {
Spilly 5:40ac894e0fa7 489 float accumulator[3] = {0,0,0};
Spilly 5:40ac894e0fa7 490 int sampleCount = 0;
Spilly 5:40ac894e0fa7 491
Spilly 5:40ac894e0fa7 492
Spilly 5:40ac894e0fa7 493 //We'll take a certain number of samples and then average them to calculate the offset
Spilly 5:40ac894e0fa7 494 while (sampleCount < AVG_COUNT)
Spilly 5:40ac894e0fa7 495 {
Spilly 5:40ac894e0fa7 496 //get gyro data
Spilly 5:40ac894e0fa7 497 gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
Spilly 5:40ac894e0fa7 498 //add current sample to previous samples
Spilly 5:40ac894e0fa7 499 accumulator[2] += degGyro[2];
Spilly 5:40ac894e0fa7 500 sampleCount++;
Spilly 5:40ac894e0fa7 501 //Make sure the gyro has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 502 wait(GYRO_SAMPLE_PERIOD);
Spilly 5:40ac894e0fa7 503 }
Spilly 5:40ac894e0fa7 504
Spilly 5:40ac894e0fa7 505
Spilly 5:40ac894e0fa7 506 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 507 degGyro[2] = accumulator[2] / AVG_COUNT;
Spilly 5:40ac894e0fa7 508
Spilly 5:40ac894e0fa7 509 }
Spilly 5:40ac894e0fa7 510
Spilly 5:40ac894e0fa7 511 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 512 // gyroCal
Spilly 5:40ac894e0fa7 513 /********************************************************************************************************************************/
Spilly 5:40ac894e0fa7 514
Spilly 5:40ac894e0fa7 515 void gyroCal()
Spilly 5:40ac894e0fa7 516 {
Spilly 5:40ac894e0fa7 517 float accumulator[3] = {0,0,0};
Spilly 5:40ac894e0fa7 518 int sampleCount = 0;
Spilly 5:40ac894e0fa7 519
Spilly 5:40ac894e0fa7 520
Spilly 5:40ac894e0fa7 521 //We'll take a certain number of samples and then average them to calculate the offset
Spilly 5:40ac894e0fa7 522 while (sampleCount < CALIBRATION_COUNT)
Spilly 5:40ac894e0fa7 523 {
Spilly 5:40ac894e0fa7 524 float gyroData[3];
Spilly 5:40ac894e0fa7 525 //Make sure the gyro has had enough time to take a new sample.
Spilly 5:40ac894e0fa7 526 wait(GYRO_SAMPLE_PERIOD);
Spilly 5:40ac894e0fa7 527 //get gyro data
Spilly 5:40ac894e0fa7 528 gyro.read(&gyroData[0],&gyroData[1],&gyroData[2]);
Spilly 5:40ac894e0fa7 529 for(int i = 0; i < 3; i++)
Spilly 5:40ac894e0fa7 530 {
Spilly 5:40ac894e0fa7 531 //add current sample to previous samples
Spilly 5:40ac894e0fa7 532 accumulator[i] += gyroData[i];
Spilly 5:40ac894e0fa7 533 }
Spilly 5:40ac894e0fa7 534 sampleCount++;
Spilly 5:40ac894e0fa7 535 }
Spilly 5:40ac894e0fa7 536
Spilly 5:40ac894e0fa7 537 for(int i = 0; i < 3; i++)
Spilly 5:40ac894e0fa7 538 {
Spilly 5:40ac894e0fa7 539 //divide by number of samples to get average offset
Spilly 5:40ac894e0fa7 540 gyroOffset[i] = accumulator[i] / CALIBRATION_COUNT;
Spilly 5:40ac894e0fa7 541 }
Spilly 5:40ac894e0fa7 542 }
Spilly 5:40ac894e0fa7 543
Spilly 5:40ac894e0fa7 544 void gyroOnly()
Spilly 5:40ac894e0fa7 545 {
Spilly 5:40ac894e0fa7 546 gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
Spilly 5:40ac894e0fa7 547 //compensate for gyro offset
Spilly 5:40ac894e0fa7 548 for(int i=0;i<=3;i++)
Spilly 5:40ac894e0fa7 549 {
Spilly 5:40ac894e0fa7 550 degGyro[i] -= gyroOffset[i];
Spilly 5:40ac894e0fa7 551 }
Spilly 5:40ac894e0fa7 552 }
Spilly 5:40ac894e0fa7 553