David Spillman / Mbed 2 deprecated GPSNavigation

Crude navigation

Dependencies:   GPS L3GD20 LSM303DLHC mbed PID

main.cpp@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 "GPS.h"
Spilly 0:e79311aae7ed 3 #include "modSensData.h"
Spilly 0:e79311aae7ed 4
Spilly 0:e79311aae7ed 5 //Radius of the earth in meters
Spilly 0:e79311aae7ed 6 #define EARTHRADIUS 6378100.0f
Spilly 0:e79311aae7ed 7 //Tolerance for heading actual and heading needed
Spilly 0:e79311aae7ed 8 #define HEADDIFF 5.0f
Spilly 0:e79311aae7ed 9 //Period in seconds of the the main loop
Spilly 0:e79311aae7ed 10 #define PERIOD 1
Spilly 0:e79311aae7ed 11
Spilly 0:e79311aae7ed 12 //GPS
Spilly 0:e79311aae7ed 13 GPS gps(D9, D7);
Spilly 0:e79311aae7ed 14
Spilly 0:e79311aae7ed 15 //X-Bee
Spilly 0:e79311aae7ed 16 Serial xBee(PTC15, PTC14);
Spilly 0:e79311aae7ed 17
Spilly 0:e79311aae7ed 18
Spilly 0:e79311aae7ed 19 float startPos[2], curPos[2], goalPos[2] = {35.478593, -81.981878}, polarVector[2];
Spilly 0:e79311aae7ed 20
Spilly 0:e79311aae7ed 21 void setGoalPos(float lat, float lon);
Spilly 0:e79311aae7ed 22 void makeVector(void);
Spilly 0:e79311aae7ed 23 char whichWay(float magHead, float calcHead);
Spilly 0:e79311aae7ed 24 void testYaw(void);
Spilly 0:e79311aae7ed 25
Spilly 0:e79311aae7ed 26 int main()
Spilly 0:e79311aae7ed 27 {
Spilly 0:e79311aae7ed 28 xBee.baud(9600);
Spilly 0:e79311aae7ed 29
Spilly 0:e79311aae7ed 30 //uncomment to check math for calculating needed heading
Spilly 0:e79311aae7ed 31 //testYaw();
Spilly 0:e79311aae7ed 32
Spilly 0:e79311aae7ed 33 xBee.printf("\nI'm Alive...\n");
Spilly 0:e79311aae7ed 34
Spilly 0:e79311aae7ed 35 //Setup the GPS
Spilly 0:e79311aae7ed 36 gps.Init();
Spilly 0:e79311aae7ed 37
Spilly 0:e79311aae7ed 38 gps.parseData();
Spilly 0:e79311aae7ed 39
Spilly 0:e79311aae7ed 40 //wait until we have a gps fix
Spilly 0:e79311aae7ed 41 while(gps.fixtype == 0)
Spilly 0:e79311aae7ed 42 {
Spilly 0:e79311aae7ed 43 xBee.printf("fix %d\n", gps.fixtype);
Spilly 0:e79311aae7ed 44 gps.parseData();
Spilly 0:e79311aae7ed 45 wait(.2);
Spilly 0:e79311aae7ed 46 }
Spilly 0:e79311aae7ed 47
Spilly 0:e79311aae7ed 48 //set starting position
Spilly 0:e79311aae7ed 49 curPos[0] = gps.latitude;
Spilly 0:e79311aae7ed 50 curPos[1] = gps.longitude;
Spilly 0:e79311aae7ed 51
Spilly 0:e79311aae7ed 52 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 53 makeVector();
Spilly 0:e79311aae7ed 54
Spilly 0:e79311aae7ed 55 //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
Spilly 0:e79311aae7ed 56 //printf("magn %f\tangle %f\tlat %f\tlon %f\tgoalLat %f\tgoalLon %f\n", polarVector[0], polarVector[1], gps.latitude, gps.longitude, goalPos[0], goalPos[1]);
Spilly 0:e79311aae7ed 57
Spilly 0:e79311aae7ed 58 while (1)
Spilly 0:e79311aae7ed 59 {
Spilly 0:e79311aae7ed 60 // Process / check GPS data
Spilly 0:e79311aae7ed 61 gps.parseData();
Spilly 0:e79311aae7ed 62 //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
Spilly 0:e79311aae7ed 63
Spilly 0:e79311aae7ed 64 //update current position
Spilly 0:e79311aae7ed 65 curPos[0] = gps.latitude;
Spilly 0:e79311aae7ed 66 curPos[1] = gps.longitude;
Spilly 0:e79311aae7ed 67
Spilly 0:e79311aae7ed 68 makeVector();
Spilly 0:e79311aae7ed 69
Spilly 0:e79311aae7ed 70 //get data from IMU and do calculations to determine heading
Spilly 0:e79311aae7ed 71 updateAngles();
Spilly 0:e79311aae7ed 72
Spilly 0:e79311aae7ed 73 //decide which way to go
Spilly 0:e79311aae7ed 74 char direction = whichWay(yaw, polarVector[1]);
Spilly 0:e79311aae7ed 75 xBee.printf("dist %f\tneed %f\tcurrent %f\tdir %c\tlat %f\tlon %f\tgoalLat %f\tgoalLon %f\n", polarVector[0], polarVector[1], yaw, direction, gps.latitude, gps.longitude, goalPos[0], goalPos[1]);
Spilly 0:e79311aae7ed 76 wait(PERIOD);
Spilly 0:e79311aae7ed 77 }
Spilly 0:e79311aae7ed 78 }
Spilly 0:e79311aae7ed 79
Spilly 0:e79311aae7ed 80 //create polar vector based on two sets of latitude and longitude
Spilly 0:e79311aae7ed 81 void makeVector(void)
Spilly 0:e79311aae7ed 82 {
Spilly 0:e79311aae7ed 83 float arcLength[2];
Spilly 0:e79311aae7ed 84
Spilly 0:e79311aae7ed 85 //arc length = radius * angle
Spilly 0:e79311aae7ed 86 //Y
Spilly 0:e79311aae7ed 87 arcLength[1] = EARTHRADIUS * (goalPos[0] - curPos[0]);
Spilly 0:e79311aae7ed 88 //X
Spilly 0:e79311aae7ed 89 arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
Spilly 0:e79311aae7ed 90
Spilly 0:e79311aae7ed 91 //arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
Spilly 0:e79311aae7ed 92 //arcLength[1] = EARTHRADIUS * (curPos[0] - goalPos[0]);
Spilly 0:e79311aae7ed 93 //calculate magnitude of vector
Spilly 0:e79311aae7ed 94 polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
Spilly 0:e79311aae7ed 95
Spilly 0:e79311aae7ed 96 //Use arcTan(-x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
Spilly 0:e79311aae7ed 97 polarVector[1] = (RADTODEGREE * (atan2(-arcLength[0], arcLength[1])));
Spilly 0:e79311aae7ed 98
Spilly 0:e79311aae7ed 99 //make negative angles positive
Spilly 0:e79311aae7ed 100 if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
Spilly 0:e79311aae7ed 101 }
Spilly 0:e79311aae7ed 102
Spilly 0:e79311aae7ed 103 //Decide which direction to turn based on current compass heading and heading required
Spilly 0:e79311aae7ed 104 char whichWay(float magHead, float calcHead)
Spilly 0:e79311aae7ed 105 {
Spilly 0:e79311aae7ed 106
Spilly 0:e79311aae7ed 107 float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
Spilly 0:e79311aae7ed 108
Spilly 0:e79311aae7ed 109 //Is the heading off enough to care?
Spilly 0:e79311aae7ed 110 if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360.0f - HEADDIFF)))
Spilly 0:e79311aae7ed 111 {
Spilly 0:e79311aae7ed 112 if(calcHead > magHead)
Spilly 0:e79311aae7ed 113 {
Spilly 0:e79311aae7ed 114 if((calcHead - magHead) < 180)
Spilly 0:e79311aae7ed 115 {
Spilly 0:e79311aae7ed 116 return 'R';
Spilly 0:e79311aae7ed 117 }
Spilly 0:e79311aae7ed 118 return 'L';
Spilly 0:e79311aae7ed 119 }
Spilly 0:e79311aae7ed 120 else if((magHead - calcHead) < 180)
Spilly 0:e79311aae7ed 121 {
Spilly 0:e79311aae7ed 122 return 'L';
Spilly 0:e79311aae7ed 123 }
Spilly 0:e79311aae7ed 124 return 'R';
Spilly 0:e79311aae7ed 125 }
Spilly 0:e79311aae7ed 126 return 'G';
Spilly 0:e79311aae7ed 127 }
Spilly 0:e79311aae7ed 128
Spilly 0:e79311aae7ed 129 //Test for verifying heading
Spilly 0:e79311aae7ed 130 void testYaw(void)
Spilly 0:e79311aae7ed 131 {
Spilly 0:e79311aae7ed 132 //set test position to indicate a necessary heading of:
Spilly 0:e79311aae7ed 133 curPos[0] = 34.833716f;
Spilly 0:e79311aae7ed 134 curPos[1] = -82.404188f;
Spilly 0:e79311aae7ed 135
Spilly 0:e79311aae7ed 136 //due north
Spilly 0:e79311aae7ed 137 goalPos[0] = 35.833716f;
Spilly 0:e79311aae7ed 138 goalPos[1] = -82.404188f;
Spilly 0:e79311aae7ed 139
Spilly 0:e79311aae7ed 140 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 141 makeVector();
Spilly 0:e79311aae7ed 142
Spilly 0:e79311aae7ed 143 xBee.printf("dist %fmeters\tangle %fdegrees due north\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 144
Spilly 0:e79311aae7ed 145 //due south
Spilly 0:e79311aae7ed 146 goalPos[0] = 33.833716f;
Spilly 0:e79311aae7ed 147 goalPos[1] = -82.404188f;
Spilly 0:e79311aae7ed 148
Spilly 0:e79311aae7ed 149
Spilly 0:e79311aae7ed 150 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 151 makeVector();
Spilly 0:e79311aae7ed 152
Spilly 0:e79311aae7ed 153 xBee.printf("dist %fmeters\tangle %fdegrees due south\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 154
Spilly 0:e79311aae7ed 155 //due east
Spilly 0:e79311aae7ed 156 goalPos[0] = 34.833716f;
Spilly 0:e79311aae7ed 157 goalPos[1] = -81.404188f;
Spilly 0:e79311aae7ed 158
Spilly 0:e79311aae7ed 159 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 160 makeVector();
Spilly 0:e79311aae7ed 161
Spilly 0:e79311aae7ed 162 xBee.printf("dist %fmeters\tangle %fdegrees due east\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 163
Spilly 0:e79311aae7ed 164
Spilly 0:e79311aae7ed 165 //due west
Spilly 0:e79311aae7ed 166 goalPos[0] = 34.833716f;
Spilly 0:e79311aae7ed 167 goalPos[1] = -83.404188f;
Spilly 0:e79311aae7ed 168
Spilly 0:e79311aae7ed 169 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 170 makeVector();
Spilly 0:e79311aae7ed 171
Spilly 0:e79311aae7ed 172 xBee.printf("dist %fmeters\tangle %fdegrees due west\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 173
Spilly 0:e79311aae7ed 174 //north east
Spilly 0:e79311aae7ed 175 goalPos[0] = 35.833716f;
Spilly 0:e79311aae7ed 176 goalPos[1] = -81.404188f;
Spilly 0:e79311aae7ed 177
Spilly 0:e79311aae7ed 178 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 179 makeVector();
Spilly 0:e79311aae7ed 180
Spilly 0:e79311aae7ed 181 xBee.printf("dist %fmeters\tangle %fdegrees north east\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 182
Spilly 0:e79311aae7ed 183
Spilly 0:e79311aae7ed 184 //north west
Spilly 0:e79311aae7ed 185 goalPos[0] = 35.833716f;
Spilly 0:e79311aae7ed 186 goalPos[1] = -83.404188f;
Spilly 0:e79311aae7ed 187
Spilly 0:e79311aae7ed 188
Spilly 0:e79311aae7ed 189 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 190 makeVector();
Spilly 0:e79311aae7ed 191
Spilly 0:e79311aae7ed 192 xBee.printf("dist %fmeters\tangle %fdegrees north west\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 193
Spilly 0:e79311aae7ed 194 //south east
Spilly 0:e79311aae7ed 195 goalPos[0] = 33.833716f;
Spilly 0:e79311aae7ed 196 goalPos[1] = -81.404188f;
Spilly 0:e79311aae7ed 197
Spilly 0:e79311aae7ed 198 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 199 makeVector();
Spilly 0:e79311aae7ed 200
Spilly 0:e79311aae7ed 201 xBee.printf("dist %fmeters\tangle %fdegrees south east\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 202
Spilly 0:e79311aae7ed 203
Spilly 0:e79311aae7ed 204 //south west
Spilly 0:e79311aae7ed 205 goalPos[0] = 33.833716f;
Spilly 0:e79311aae7ed 206 goalPos[1] = -83.404188f;
Spilly 0:e79311aae7ed 207
Spilly 0:e79311aae7ed 208
Spilly 0:e79311aae7ed 209 //Convert starting position and goal position to a vector
Spilly 0:e79311aae7ed 210 makeVector();
Spilly 0:e79311aae7ed 211
Spilly 0:e79311aae7ed 212 xBee.printf("dist %fmeters\tangle %fdegrees south west\n", polarVector[0], polarVector[1]);
Spilly 0:e79311aae7ed 213
Spilly 0:e79311aae7ed 214 while(1);
Spilly 0:e79311aae7ed 215
Spilly 0:e79311aae7ed 216 /*
Spilly 0:e79311aae7ed 217 //below is useful when checking compass heading
Spilly 0:e79311aae7ed 218 while(1)
Spilly 0:e79311aae7ed 219 {
Spilly 0:e79311aae7ed 220 updateAngles();
Spilly 0:e79311aae7ed 221 char direction = whichWay(yaw, polarVector[1]);
Spilly 0:e79311aae7ed 222 //printf("accX %f\taccY %f\taccZ %f\tmagX %f\tmagY %f\tmagZ %f\troll %f\tpitch %f\tyaw %f\tturn %c\n", accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2], roll, pitch, yaw, direction);
Spilly 0:e79311aae7ed 223 wait(1);
Spilly 0:e79311aae7ed 224 }
Spilly 0:e79311aae7ed 225 */
Spilly 0:e79311aae7ed 226 }