David Spillman / Mbed 2 deprecated GPSNavigationNew

Crude navigation

Dependencies:   GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem

Fork of GPSNavigation by David Spillman

modSensData.h@4:a397b44a0fe8, 2015-03-17 (annotated)

Committer:
Spilly
Date:
Tue Mar 17 01:13:17 2015 +0000
Revision:
4:a397b44a0fe8
Parent:
3:ffa0e1429a72
Child:
5:40ac894e0fa7
Accel and magn offsets

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Spilly 0:e79311aae7ed 1 #include "mbed.h"
Spilly 0:e79311aae7ed 2 #include "math.h"
Spilly 0:e79311aae7ed 3 #include "L3GD20.h"
Spilly 0:e79311aae7ed 4 #include "LSM303DLHC.h"
Spilly 0:e79311aae7ed 5
Spilly 0:e79311aae7ed 6 L3GD20 gyro(PTE25, PTE24);
Spilly 0:e79311aae7ed 7 LSM303DLHC compass(PTE25, PTE24);
Spilly 0:e79311aae7ed 8
Spilly 0:e79311aae7ed 9 #define M_PI 3.1415926535897932384626433832795f
Spilly 0:e79311aae7ed 10 #define TWO_PI 6.283185307179586476925286766559f
Spilly 4:a397b44a0fe8 11 #define RADTODEGREE 57.295779513082320876798154814105f
Spilly 4:a397b44a0fe8 12 #define GRAVITY 1.0f
Spilly 4:a397b44a0fe8 13 #define THRESHOLD 1.1f
Spilly 0:e79311aae7ed 14
Spilly 4:a397b44a0fe8 15 // "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
Spilly 4:a397b44a0fe8 16 #define ACCEL_X_MIN ((float) -276)
Spilly 4:a397b44a0fe8 17 #define ACCEL_X_MAX ((float) 236)
Spilly 4:a397b44a0fe8 18 #define ACCEL_Y_MIN ((float) -261)
Spilly 4:a397b44a0fe8 19 #define ACCEL_Y_MAX ((float) 245)
Spilly 4:a397b44a0fe8 20 #define ACCEL_Z_MIN ((float) -284)
Spilly 4:a397b44a0fe8 21 #define ACCEL_Z_MAX ((float) 220)
Spilly 0:e79311aae7ed 22
Spilly 4:a397b44a0fe8 23 // "magn x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
Spilly 4:a397b44a0fe8 24 #define MAGN_X_MIN (-0.370909f)
Spilly 4:a397b44a0fe8 25 #define MAGN_X_MAX (0.569091f)
Spilly 4:a397b44a0fe8 26 #define MAGN_Y_MIN (-0.516364f)
Spilly 4:a397b44a0fe8 27 #define MAGN_Y_MAX (0.392727f)
Spilly 4:a397b44a0fe8 28 #define MAGN_Z_MIN (-0.618367f)
Spilly 4:a397b44a0fe8 29 #define MAGN_Z_MAX (0.408163f)
Spilly 0:e79311aae7ed 30
Spilly 4:a397b44a0fe8 31 // Sensor calibration scale and offset values
Spilly 4:a397b44a0fe8 32 #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 33 #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 34 #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 35 #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
Spilly 4:a397b44a0fe8 36 #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
Spilly 4:a397b44a0fe8 37 #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
Spilly 4:a397b44a0fe8 38
Spilly 4:a397b44a0fe8 39 #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 40 #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 41 #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
Spilly 4:a397b44a0fe8 42 #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
Spilly 4:a397b44a0fe8 43 #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
Spilly 4:a397b44a0fe8 44 #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
Spilly 4:a397b44a0fe8 45
Spilly 4:a397b44a0fe8 46
Spilly 4:a397b44a0fe8 47
Spilly 4:a397b44a0fe8 48 //calibrated reading = (reading - offset) * scale
Spilly 2:503a5ac6c3b6 49
Spilly 0:e79311aae7ed 50 Timer t1;
Spilly 0:e79311aae7ed 51
Spilly 0:e79311aae7ed 52 float vC[3], cC[3], magnetom[3], inertialAccel[3];
Spilly 0:e79311aae7ed 53
Spilly 0:e79311aae7ed 54 float aP[3], vP[3], cP[3], deltaT, timeStamp, degGyro[3], angPos[3], prevAngPos[3];
Spilly 0:e79311aae7ed 55
Spilly 0:e79311aae7ed 56 double roll = 0, pitch = 0, yaw = 0;
Spilly 0:e79311aae7ed 57
Spilly 0:e79311aae7ed 58 float magCheck;
Spilly 0:e79311aae7ed 59
Spilly 0:e79311aae7ed 60 float accel[3], bias[3], magnDiff;
Spilly 0:e79311aae7ed 61
Spilly 0:e79311aae7ed 62 int printStamp = 0, stopStamp = 0, dangerAhead = 0, howLong = 0, count[3], pos[5][3]= { {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0} };
Spilly 0:e79311aae7ed 63
Spilly 0:e79311aae7ed 64 int flag = 0, signBit = 0;
Spilly 0:e79311aae7ed 65
Spilly 0:e79311aae7ed 66 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 67 // Get Accelerometer and Magnetometer Data
Spilly 0:e79311aae7ed 68 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 69
Spilly 0:e79311aae7ed 70 void readIMU()
Spilly 0:e79311aae7ed 71 {
Spilly 0:e79311aae7ed 72 compass.read(&accel[0], &accel[1], &accel[2], &magnetom[0], &magnetom[1], &magnetom[2]);
Spilly 4:a397b44a0fe8 73 //Compensate magnetometer error
Spilly 4:a397b44a0fe8 74 //magnetom[0] -= MAGN_X_OFFSET;
Spilly 4:a397b44a0fe8 75 //magnetom[1] -= MAGN_Y_OFFSET;
Spilly 4:a397b44a0fe8 76 //magnetom[2] -= MAGN_Z_OFFSET;
Spilly 4:a397b44a0fe8 77
Spilly 4:a397b44a0fe8 78 //Compensate accelerometer error
Spilly 4:a397b44a0fe8 79 //accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
Spilly 4:a397b44a0fe8 80 //accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
Spilly 4:a397b44a0fe8 81 //accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
Spilly 4:a397b44a0fe8 82
Spilly 0:e79311aae7ed 83 gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
Spilly 0:e79311aae7ed 84 }
Spilly 0:e79311aae7ed 85
Spilly 0:e79311aae7ed 86 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 87 // Compass Heading
Spilly 0:e79311aae7ed 88 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 89
Spilly 0:e79311aae7ed 90 void Compass_Heading()
Spilly 0:e79311aae7ed 91 {
Spilly 0:e79311aae7ed 92 float mag_x;
Spilly 0:e79311aae7ed 93 float mag_y;
Spilly 0:e79311aae7ed 94 float cos_roll;
Spilly 0:e79311aae7ed 95 float sin_roll;
Spilly 0:e79311aae7ed 96 float cos_pitch;
Spilly 0:e79311aae7ed 97 float sin_pitch;
Spilly 0:e79311aae7ed 98
Spilly 0:e79311aae7ed 99 //saves a few processor cycles by calculating and storing values
Spilly 0:e79311aae7ed 100 cos_roll = cos(roll);
Spilly 0:e79311aae7ed 101 sin_roll = sin(roll);
Spilly 0:e79311aae7ed 102 cos_pitch = cos(pitch);
Spilly 0:e79311aae7ed 103 sin_pitch = sin(pitch);
Spilly 0:e79311aae7ed 104
Spilly 0:e79311aae7ed 105 // Tilt compensated magnetic field X
Spilly 0:e79311aae7ed 106 mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
Spilly 0:e79311aae7ed 107 // Tilt compensated magnetic field Y
Spilly 0:e79311aae7ed 108 mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
Spilly 0:e79311aae7ed 109 // Magnetic Heading
Spilly 0:e79311aae7ed 110 yaw = atan2(-mag_x, mag_y);
Spilly 0:e79311aae7ed 111
Spilly 0:e79311aae7ed 112 //make negative angles positive
Spilly 0:e79311aae7ed 113 if(yaw < 0) yaw = yaw + TWO_PI;
Spilly 0:e79311aae7ed 114
Spilly 0:e79311aae7ed 115 yaw = yaw * RADTODEGREE;
Spilly 0:e79311aae7ed 116 }
Spilly 0:e79311aae7ed 117
Spilly 0:e79311aae7ed 118 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 119 // getAttitude
Spilly 0:e79311aae7ed 120 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 121
Spilly 0:e79311aae7ed 122 void getAttitude()
Spilly 0:e79311aae7ed 123 {
Spilly 0:e79311aae7ed 124 float temp1[3];
Spilly 0:e79311aae7ed 125 float temp2[3];
Spilly 0:e79311aae7ed 126 float xAxis[3] = {1.0f, 0.0f, 0.0f};
Spilly 0:e79311aae7ed 127
Spilly 0:e79311aae7ed 128 // GET PITCH
Spilly 0:e79311aae7ed 129 // Using y-z-plane-component/x-component of gravity vector
Spilly 0:e79311aae7ed 130 pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
Spilly 0:e79311aae7ed 131
Spilly 0:e79311aae7ed 132 //printf("P = %f", pitch);
Spilly 0:e79311aae7ed 133
Spilly 0:e79311aae7ed 134 // GET ROLL
Spilly 0:e79311aae7ed 135 // Compensate pitch of gravity vector
Spilly 0:e79311aae7ed 136 temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
Spilly 0:e79311aae7ed 137 temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
Spilly 0:e79311aae7ed 138 temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
Spilly 0:e79311aae7ed 139
Spilly 0:e79311aae7ed 140 temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
Spilly 0:e79311aae7ed 141 temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
Spilly 0:e79311aae7ed 142 temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
Spilly 0:e79311aae7ed 143
Spilly 0:e79311aae7ed 144 // Since we compensated for pitch, x-z-plane-component equals z-component:
Spilly 0:e79311aae7ed 145 roll = atan2(temp2[1], temp2[2]);
Spilly 0:e79311aae7ed 146 }
Spilly 0:e79311aae7ed 147
Spilly 0:e79311aae7ed 148 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 149 // rotateBTI
Spilly 0:e79311aae7ed 150 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 151
Spilly 0:e79311aae7ed 152 //rotation from the body frame to the inertial frame
Spilly 0:e79311aae7ed 153 void rotateBTI()
Spilly 0:e79311aae7ed 154 {
Spilly 0:e79311aae7ed 155 float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
Spilly 0:e79311aae7ed 156
Spilly 0:e79311aae7ed 157 float gravity[3] = {0, 0, -GRAVITY};
Spilly 0:e79311aae7ed 158
Spilly 0:e79311aae7ed 159 float rIB[3][3];
Spilly 0:e79311aae7ed 160
Spilly 0:e79311aae7ed 161 //saves a few processor cycles by calculating and storing values
Spilly 0:e79311aae7ed 162
Spilly 0:e79311aae7ed 163 //Serial.print("Roll");
Spilly 0:e79311aae7ed 164 cos_roll = cos(roll);
Spilly 0:e79311aae7ed 165 //Serial.print(cos_roll, 4);
Spilly 0:e79311aae7ed 166 //Serial.print(" ");
Spilly 0:e79311aae7ed 167 sin_roll = sin(roll);
Spilly 0:e79311aae7ed 168 //Serial.print(sin_roll, 4);
Spilly 0:e79311aae7ed 169 //Serial.print(" ");
Spilly 0:e79311aae7ed 170
Spilly 0:e79311aae7ed 171 //Serial.print("Pitch");
Spilly 0:e79311aae7ed 172 cos_pitch = cos(pitch);
Spilly 0:e79311aae7ed 173 //Serial.print(cos_pitch, 4);
Spilly 0:e79311aae7ed 174 //Serial.print(" ");
Spilly 0:e79311aae7ed 175 sin_pitch = sin(pitch);
Spilly 0:e79311aae7ed 176 //Serial.print(sin_pitch, 4);
Spilly 0:e79311aae7ed 177 //Serial.print(" ");
Spilly 0:e79311aae7ed 178
Spilly 0:e79311aae7ed 179 //Serial.print("YAW");
Spilly 0:e79311aae7ed 180 cos_yaw = cos(yaw);
Spilly 0:e79311aae7ed 181 //Serial.print(cos_yaw, 4);
Spilly 0:e79311aae7ed 182 //Serial.print(" ");
Spilly 0:e79311aae7ed 183 sin_yaw = sin(yaw);
Spilly 0:e79311aae7ed 184 //Serial.print(sin_yaw, 4);
Spilly 0:e79311aae7ed 185 //Serial.print(" ");
Spilly 0:e79311aae7ed 186
Spilly 0:e79311aae7ed 187 rIB[0][0] = cos_yaw * cos_pitch;
Spilly 0:e79311aae7ed 188 rIB[0][1] = (cos_yaw * sin_roll * sin_pitch) - (cos_roll * sin_yaw);
Spilly 0:e79311aae7ed 189 rIB[0][2] = (sin_roll * sin_yaw) + (cos_roll * cos_yaw * sin_pitch);
Spilly 0:e79311aae7ed 190 rIB[1][0] = cos_pitch * sin_yaw;
Spilly 0:e79311aae7ed 191 rIB[1][1] = (cos_roll * cos_yaw) + (sin_roll * sin_yaw * sin_pitch);
Spilly 0:e79311aae7ed 192 rIB[1][2] = (cos_roll * sin_yaw * sin_pitch) - (cos_yaw * sin_roll);
Spilly 0:e79311aae7ed 193 rIB[2][0] = -1 * sin_pitch;
Spilly 0:e79311aae7ed 194 rIB[2][1] = cos_pitch * sin_roll;
Spilly 0:e79311aae7ed 195 rIB[2][2] = cos_roll * cos_pitch;
Spilly 0:e79311aae7ed 196
Spilly 0:e79311aae7ed 197 for(int i = 0; i < 3; i++)
Spilly 0:e79311aae7ed 198 {
Spilly 0:e79311aae7ed 199 aP[i] = inertialAccel[i];
Spilly 0:e79311aae7ed 200 inertialAccel[i] = rIB[i][0] * accel[0] + rIB[i][1] * accel[1] + rIB[i][2] * accel[2];
Spilly 0:e79311aae7ed 201 inertialAccel[i] = inertialAccel[i] + gravity[i];
Spilly 0:e79311aae7ed 202 }
Spilly 0:e79311aae7ed 203 }
Spilly 0:e79311aae7ed 204
Spilly 0:e79311aae7ed 205
Spilly 0:e79311aae7ed 206
Spilly 2:503a5ac6c3b6 207 int updateAngles()
Spilly 0:e79311aae7ed 208 {
Spilly 0:e79311aae7ed 209 readIMU();
Spilly 3:ffa0e1429a72 210 if((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * accel[2]) <= THRESHOLD))
Spilly 2:503a5ac6c3b6 211 {
Spilly 2:503a5ac6c3b6 212 //float prevYaw = yaw;
Spilly 2:503a5ac6c3b6 213 getAttitude();
Spilly 2:503a5ac6c3b6 214 Compass_Heading();
Spilly 2:503a5ac6c3b6 215 //float absOfYawDiff = sqrt((prevYaw - yaw) * (prevYaw - yaw));
Spilly 2:503a5ac6c3b6 216 //check for noise
Spilly 2:503a5ac6c3b6 217 //if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
Spilly 2:503a5ac6c3b6 218 //{
Spilly 2:503a5ac6c3b6 219 return 1;
Spilly 2:503a5ac6c3b6 220 //}
Spilly 2:503a5ac6c3b6 221 //return 0;
Spilly 2:503a5ac6c3b6 222
Spilly 2:503a5ac6c3b6 223 }
Spilly 2:503a5ac6c3b6 224 return 0;
Spilly 0:e79311aae7ed 225 //rotateBTI();
Spilly 2:503a5ac6c3b6 226 }
Spilly 2:503a5ac6c3b6 227
Spilly 2:503a5ac6c3b6 228 float getGyro()
Spilly 2:503a5ac6c3b6 229 {
Spilly 2:503a5ac6c3b6 230 float gx, gy, gz;
Spilly 2:503a5ac6c3b6 231 gyro.read(&gx,&gy,&gz);
Spilly 2:503a5ac6c3b6 232 return gz;
Spilly 0:e79311aae7ed 233 }