David Spillman / Mbed 2 deprecated GPSNavigation

Crude navigation

Dependencies:   GPS L3GD20 LSM303DLHC mbed PID

modSensData.h@0:e79311aae7ed, 2015-02-21 (annotated)

Committer:
Spilly
Date:
Sat Feb 21 01:14:46 2015 +0000
Revision:
0:e79311aae7ed
Child:
2:503a5ac6c3b6
Crude GPS navigation.  Determines whether to turn right, left, or go straight based on goal position, current position, and heading.

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