David Spillman / Mbed 2 deprecated GPSNavigation

Crude navigation

Dependencies:   GPS L3GD20 LSM303DLHC mbed PID

modSensData.h@2:503a5ac6c3b6, 2015-03-04 (annotated)

Committer:
Spilly
Date:
Wed Mar 04 17:48:08 2015 +0000
Revision:
2:503a5ac6c3b6
Parent:
0:e79311aae7ed
Child:
3:ffa0e1429a72
Implemented PID

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 XBIAS 0
Spilly 0:e79311aae7ed 10 #define YBIAS 0
Spilly 0:e79311aae7ed 11 #define ZBIAS 0
Spilly 0:e79311aae7ed 12
Spilly 0:e79311aae7ed 13 #define M_PI 3.1415926535897932384626433832795f
Spilly 0:e79311aae7ed 14 #define TWO_PI 6.283185307179586476925286766559f
Spilly 0:e79311aae7ed 15
Spilly 0:e79311aae7ed 16 #define RADTODEGREE 57.295779513082320876798154814105f
Spilly 0:e79311aae7ed 17
Spilly 0:e79311aae7ed 18 #define GRAVITY 1.0f
Spilly 0:e79311aae7ed 19
Spilly 2:503a5ac6c3b6 20 #define THRESHOLD 1.1f
Spilly 2:503a5ac6c3b6 21
Spilly 0:e79311aae7ed 22 Timer t1;
Spilly 0:e79311aae7ed 23
Spilly 0:e79311aae7ed 24 float vC[3], cC[3], magnetom[3], inertialAccel[3];
Spilly 0:e79311aae7ed 25
Spilly 0:e79311aae7ed 26 float aP[3], vP[3], cP[3], deltaT, timeStamp, degGyro[3], angPos[3], prevAngPos[3];
Spilly 0:e79311aae7ed 27
Spilly 0:e79311aae7ed 28 double roll = 0, pitch = 0, yaw = 0;
Spilly 0:e79311aae7ed 29
Spilly 0:e79311aae7ed 30 float magCheck;
Spilly 0:e79311aae7ed 31
Spilly 0:e79311aae7ed 32 float accel[3], bias[3], magnDiff;
Spilly 0:e79311aae7ed 33
Spilly 0:e79311aae7ed 34 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 35
Spilly 0:e79311aae7ed 36 int flag = 0, signBit = 0;
Spilly 0:e79311aae7ed 37
Spilly 0:e79311aae7ed 38 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 39 // Get Accelerometer and Magnetometer Data
Spilly 0:e79311aae7ed 40 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 41
Spilly 0:e79311aae7ed 42 void readIMU()
Spilly 0:e79311aae7ed 43 {
Spilly 0:e79311aae7ed 44 compass.read(&accel[0], &accel[1], &accel[2], &magnetom[0], &magnetom[1], &magnetom[2]);
Spilly 0:e79311aae7ed 45 gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
Spilly 0:e79311aae7ed 46 }
Spilly 0:e79311aae7ed 47
Spilly 0:e79311aae7ed 48 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 49 // Compass Heading
Spilly 0:e79311aae7ed 50 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 51
Spilly 0:e79311aae7ed 52 void Compass_Heading()
Spilly 0:e79311aae7ed 53 {
Spilly 0:e79311aae7ed 54 float mag_x;
Spilly 0:e79311aae7ed 55 float mag_y;
Spilly 0:e79311aae7ed 56 float cos_roll;
Spilly 0:e79311aae7ed 57 float sin_roll;
Spilly 0:e79311aae7ed 58 float cos_pitch;
Spilly 0:e79311aae7ed 59 float sin_pitch;
Spilly 0:e79311aae7ed 60
Spilly 0:e79311aae7ed 61 //saves a few processor cycles by calculating and storing values
Spilly 0:e79311aae7ed 62 cos_roll = cos(roll);
Spilly 0:e79311aae7ed 63 sin_roll = sin(roll);
Spilly 0:e79311aae7ed 64 cos_pitch = cos(pitch);
Spilly 0:e79311aae7ed 65 sin_pitch = sin(pitch);
Spilly 0:e79311aae7ed 66
Spilly 0:e79311aae7ed 67 // Tilt compensated magnetic field X
Spilly 0:e79311aae7ed 68 mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
Spilly 0:e79311aae7ed 69 // Tilt compensated magnetic field Y
Spilly 0:e79311aae7ed 70 mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
Spilly 0:e79311aae7ed 71 // Magnetic Heading
Spilly 0:e79311aae7ed 72 yaw = atan2(-mag_x, mag_y);
Spilly 0:e79311aae7ed 73
Spilly 0:e79311aae7ed 74 //make negative angles positive
Spilly 0:e79311aae7ed 75 if(yaw < 0) yaw = yaw + TWO_PI;
Spilly 0:e79311aae7ed 76
Spilly 0:e79311aae7ed 77 yaw = yaw * RADTODEGREE;
Spilly 0:e79311aae7ed 78 }
Spilly 0:e79311aae7ed 79
Spilly 0:e79311aae7ed 80 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 81 // getAttitude
Spilly 0:e79311aae7ed 82 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 83
Spilly 0:e79311aae7ed 84 void getAttitude()
Spilly 0:e79311aae7ed 85 {
Spilly 0:e79311aae7ed 86 float temp1[3];
Spilly 0:e79311aae7ed 87 float temp2[3];
Spilly 0:e79311aae7ed 88 float xAxis[3] = {1.0f, 0.0f, 0.0f};
Spilly 0:e79311aae7ed 89
Spilly 0:e79311aae7ed 90 // GET PITCH
Spilly 0:e79311aae7ed 91 // Using y-z-plane-component/x-component of gravity vector
Spilly 0:e79311aae7ed 92 pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
Spilly 0:e79311aae7ed 93
Spilly 0:e79311aae7ed 94 //printf("P = %f", pitch);
Spilly 0:e79311aae7ed 95
Spilly 0:e79311aae7ed 96 // GET ROLL
Spilly 0:e79311aae7ed 97 // Compensate pitch of gravity vector
Spilly 0:e79311aae7ed 98 temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
Spilly 0:e79311aae7ed 99 temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
Spilly 0:e79311aae7ed 100 temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
Spilly 0:e79311aae7ed 101
Spilly 0:e79311aae7ed 102 temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
Spilly 0:e79311aae7ed 103 temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
Spilly 0:e79311aae7ed 104 temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
Spilly 0:e79311aae7ed 105
Spilly 0:e79311aae7ed 106 // Since we compensated for pitch, x-z-plane-component equals z-component:
Spilly 0:e79311aae7ed 107 roll = atan2(temp2[1], temp2[2]);
Spilly 0:e79311aae7ed 108 }
Spilly 0:e79311aae7ed 109
Spilly 0:e79311aae7ed 110
Spilly 0:e79311aae7ed 111 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 112 // compensateAcc
Spilly 0:e79311aae7ed 113 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 114 /*
Spilly 0:e79311aae7ed 115 void compensateAcc()
Spilly 0:e79311aae7ed 116 {
Spilly 0:e79311aae7ed 117 // Compensate accelerometer error
Spilly 0:e79311aae7ed 118 accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
Spilly 0:e79311aae7ed 119 accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
Spilly 0:e79311aae7ed 120 accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
Spilly 0:e79311aae7ed 121 }
Spilly 0:e79311aae7ed 122 */
Spilly 0:e79311aae7ed 123
Spilly 0:e79311aae7ed 124 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 125 // rotateBTI
Spilly 0:e79311aae7ed 126 /********************************************************************************************************************************/
Spilly 0:e79311aae7ed 127
Spilly 0:e79311aae7ed 128 //rotation from the body frame to the inertial frame
Spilly 0:e79311aae7ed 129 void rotateBTI()
Spilly 0:e79311aae7ed 130 {
Spilly 0:e79311aae7ed 131 float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
Spilly 0:e79311aae7ed 132
Spilly 0:e79311aae7ed 133 float gravity[3] = {0, 0, -GRAVITY};
Spilly 0:e79311aae7ed 134
Spilly 0:e79311aae7ed 135 float rIB[3][3];
Spilly 0:e79311aae7ed 136
Spilly 0:e79311aae7ed 137 //saves a few processor cycles by calculating and storing values
Spilly 0:e79311aae7ed 138
Spilly 0:e79311aae7ed 139 //Serial.print("Roll");
Spilly 0:e79311aae7ed 140 cos_roll = cos(roll);
Spilly 0:e79311aae7ed 141 //Serial.print(cos_roll, 4);
Spilly 0:e79311aae7ed 142 //Serial.print(" ");
Spilly 0:e79311aae7ed 143 sin_roll = sin(roll);
Spilly 0:e79311aae7ed 144 //Serial.print(sin_roll, 4);
Spilly 0:e79311aae7ed 145 //Serial.print(" ");
Spilly 0:e79311aae7ed 146
Spilly 0:e79311aae7ed 147 //Serial.print("Pitch");
Spilly 0:e79311aae7ed 148 cos_pitch = cos(pitch);
Spilly 0:e79311aae7ed 149 //Serial.print(cos_pitch, 4);
Spilly 0:e79311aae7ed 150 //Serial.print(" ");
Spilly 0:e79311aae7ed 151 sin_pitch = sin(pitch);
Spilly 0:e79311aae7ed 152 //Serial.print(sin_pitch, 4);
Spilly 0:e79311aae7ed 153 //Serial.print(" ");
Spilly 0:e79311aae7ed 154
Spilly 0:e79311aae7ed 155 //Serial.print("YAW");
Spilly 0:e79311aae7ed 156 cos_yaw = cos(yaw);
Spilly 0:e79311aae7ed 157 //Serial.print(cos_yaw, 4);
Spilly 0:e79311aae7ed 158 //Serial.print(" ");
Spilly 0:e79311aae7ed 159 sin_yaw = sin(yaw);
Spilly 0:e79311aae7ed 160 //Serial.print(sin_yaw, 4);
Spilly 0:e79311aae7ed 161 //Serial.print(" ");
Spilly 0:e79311aae7ed 162
Spilly 0:e79311aae7ed 163 rIB[0][0] = cos_yaw * cos_pitch;
Spilly 0:e79311aae7ed 164 rIB[0][1] = (cos_yaw * sin_roll * sin_pitch) - (cos_roll * sin_yaw);
Spilly 0:e79311aae7ed 165 rIB[0][2] = (sin_roll * sin_yaw) + (cos_roll * cos_yaw * sin_pitch);
Spilly 0:e79311aae7ed 166 rIB[1][0] = cos_pitch * sin_yaw;
Spilly 0:e79311aae7ed 167 rIB[1][1] = (cos_roll * cos_yaw) + (sin_roll * sin_yaw * sin_pitch);
Spilly 0:e79311aae7ed 168 rIB[1][2] = (cos_roll * sin_yaw * sin_pitch) - (cos_yaw * sin_roll);
Spilly 0:e79311aae7ed 169 rIB[2][0] = -1 * sin_pitch;
Spilly 0:e79311aae7ed 170 rIB[2][1] = cos_pitch * sin_roll;
Spilly 0:e79311aae7ed 171 rIB[2][2] = cos_roll * cos_pitch;
Spilly 0:e79311aae7ed 172
Spilly 0:e79311aae7ed 173 for(int i = 0; i < 3; i++)
Spilly 0:e79311aae7ed 174 {
Spilly 0:e79311aae7ed 175 aP[i] = inertialAccel[i];
Spilly 0:e79311aae7ed 176 inertialAccel[i] = rIB[i][0] * accel[0] + rIB[i][1] * accel[1] + rIB[i][2] * accel[2];
Spilly 0:e79311aae7ed 177 inertialAccel[i] = inertialAccel[i] + gravity[i];
Spilly 0:e79311aae7ed 178 }
Spilly 0:e79311aae7ed 179 }
Spilly 0:e79311aae7ed 180
Spilly 0:e79311aae7ed 181
Spilly 0:e79311aae7ed 182
Spilly 2:503a5ac6c3b6 183 int updateAngles()
Spilly 0:e79311aae7ed 184 {
Spilly 0:e79311aae7ed 185 readIMU();
Spilly 2:503a5ac6c3b6 186 if((accel[0] <= THRESHOLD) && (accel[1] <= THRESHOLD) && (accel[2] <= THRESHOLD))
Spilly 2:503a5ac6c3b6 187 {
Spilly 2:503a5ac6c3b6 188 //float prevYaw = yaw;
Spilly 2:503a5ac6c3b6 189 getAttitude();
Spilly 2:503a5ac6c3b6 190 Compass_Heading();
Spilly 2:503a5ac6c3b6 191 //float absOfYawDiff = sqrt((prevYaw - yaw) * (prevYaw - yaw));
Spilly 2:503a5ac6c3b6 192 //check for noise
Spilly 2:503a5ac6c3b6 193 //if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
Spilly 2:503a5ac6c3b6 194 //{
Spilly 2:503a5ac6c3b6 195 return 1;
Spilly 2:503a5ac6c3b6 196 //}
Spilly 2:503a5ac6c3b6 197 //return 0;
Spilly 2:503a5ac6c3b6 198
Spilly 2:503a5ac6c3b6 199 }
Spilly 2:503a5ac6c3b6 200 return 0;
Spilly 0:e79311aae7ed 201 //rotateBTI();
Spilly 2:503a5ac6c3b6 202 }
Spilly 2:503a5ac6c3b6 203
Spilly 2:503a5ac6c3b6 204 float getGyro()
Spilly 2:503a5ac6c3b6 205 {
Spilly 2:503a5ac6c3b6 206 float gx, gy, gz;
Spilly 2:503a5ac6c3b6 207 gyro.read(&gx,&gy,&gz);
Spilly 2:503a5ac6c3b6 208 return gz;
Spilly 0:e79311aae7ed 209 }