Yelfie / LocalPositionSystem

Trilateration Based Local Position System

Dependents:   TDP_main_BartFork TDP_main TDP_main TDP_main_fork

LPS.cpp@2:21ca29888540, 2015-03-11 (annotated)

Committer:
Joseph_Penikis
Date:
Wed Mar 11 01:20:56 2015 +0000
Revision:
2:21ca29888540
Parent:
1:b71a1c39d263
Child:
3:dd68ac680416
Something something, I've forgotten if this is committed...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joseph_Penikis 1:b71a1c39d263 1 #include "LPS.h"
Joseph_Penikis 0:dda98f02d71e 2
Joseph_Penikis 0:dda98f02d71e 3 // Defines the delay experienced between transmitting IR and Ultrasonic pulse
Joseph_Penikis 0:dda98f02d71e 4 #define PING_OFFSET 0
Joseph_Penikis 0:dda98f02d71e 5 #define SPEED_OF_SOUND 1.0f
Joseph_Penikis 0:dda98f02d71e 6 // Used to rectify the issue of measured spheres not intersecting, may not be needed, but available anyway
Joseph_Penikis 0:dda98f02d71e 7 #define SWELL_VALUE 0
Joseph_Penikis 0:dda98f02d71e 8
Joseph_Penikis 1:b71a1c39d263 9 LPS::LPS() {}
Joseph_Penikis 1:b71a1c39d263 10 LPS::~LPS() {}
Joseph_Penikis 0:dda98f02d71e 11
Joseph_Penikis 1:b71a1c39d263 12 _3D_Vector LPS::getUnitX() { return unitX; }
Joseph_Penikis 1:b71a1c39d263 13 _3D_Vector LPS::getUnitY() { return unitY; }
Joseph_Penikis 1:b71a1c39d263 14 _3D_Vector LPS::getUnitZ() { return unitZ; }
Joseph_Penikis 1:b71a1c39d263 15 float LPS::getd() { return d; }
Joseph_Penikis 1:b71a1c39d263 16 float LPS::geti() { return i; }
Joseph_Penikis 1:b71a1c39d263 17 float LPS::getj() { return j; }
Joseph_Penikis 0:dda98f02d71e 18
Joseph_Penikis 1:b71a1c39d263 19 void LPS::updateLocation() {
Joseph_Penikis 0:dda98f02d71e 20 updateDistances();
Joseph_Penikis 0:dda98f02d71e 21
Joseph_Penikis 0:dda98f02d71e 22 current_1.x = calcX(beacon_1_distance, beacon_2_distance, d);
Joseph_Penikis 0:dda98f02d71e 23 current_1.y = calcY(beacon_1_distance, beacon_3_distance, i, j, current_1.x);
Joseph_Penikis 0:dda98f02d71e 24 current_1.z = calcZ(beacon_1_distance, current_1.x, current_1.y);
Joseph_Penikis 0:dda98f02d71e 25
Joseph_Penikis 0:dda98f02d71e 26 _3D_Vector scaledX = scaleVector(unitX, current_1.x);
Joseph_Penikis 0:dda98f02d71e 27 _3D_Vector scaledY = scaleVector(unitY, current_1.y);
Joseph_Penikis 0:dda98f02d71e 28 _3D_Vector scaledZ = scaleVector(unitZ, current_1.z);
Joseph_Penikis 0:dda98f02d71e 29
Joseph_Penikis 0:dda98f02d71e 30 current_1.x = beacon_1_loc.x + scaledX.x + scaledY.x;
Joseph_Penikis 0:dda98f02d71e 31 current_1.y = beacon_1_loc.y + scaledX.y + scaledY.y;
Joseph_Penikis 0:dda98f02d71e 32 current_1.z = beacon_1_loc.z + scaledX.z + scaledY.z;
Joseph_Penikis 0:dda98f02d71e 33
Joseph_Penikis 0:dda98f02d71e 34 current_2.x = current_1.x - scaledZ.x;
Joseph_Penikis 0:dda98f02d71e 35 current_2.y = current_1.y - scaledZ.y;
Joseph_Penikis 0:dda98f02d71e 36 current_2.z = current_1.z - scaledZ.z;
Joseph_Penikis 0:dda98f02d71e 37
Joseph_Penikis 0:dda98f02d71e 38 current_1.x += scaledZ.x;
Joseph_Penikis 0:dda98f02d71e 39 current_1.y += scaledZ.y;
Joseph_Penikis 0:dda98f02d71e 40 current_1.z += scaledZ.z;
Joseph_Penikis 0:dda98f02d71e 41
Joseph_Penikis 0:dda98f02d71e 42 // Set location closest to z=0 as location, (We are typically on the floor, but need to verify this solution works)
Joseph_Penikis 0:dda98f02d71e 43 // If current_2.z is smaller than current_1.z, swap them
Joseph_Penikis 0:dda98f02d71e 44 if (abs(current_1.z) > abs(current_2.z)) {
Joseph_Penikis 0:dda98f02d71e 45 _3D_Vector temp = current_1;
Joseph_Penikis 0:dda98f02d71e 46 current_1.x = current_2.x;
Joseph_Penikis 0:dda98f02d71e 47 current_1.y = current_2.y;
Joseph_Penikis 0:dda98f02d71e 48 current_1.z = current_2.z;
Joseph_Penikis 0:dda98f02d71e 49
Joseph_Penikis 0:dda98f02d71e 50 current_2.x = temp.x;
Joseph_Penikis 0:dda98f02d71e 51 current_2.y = temp.y;
Joseph_Penikis 0:dda98f02d71e 52 current_2.z = temp.z;
Joseph_Penikis 0:dda98f02d71e 53 }
Joseph_Penikis 0:dda98f02d71e 54
Joseph_Penikis 0:dda98f02d71e 55 // Both solutions are currently preserved incase of neccisity in future use
Joseph_Penikis 0:dda98f02d71e 56 }
Joseph_Penikis 0:dda98f02d71e 57
Joseph_Penikis 1:b71a1c39d263 58 void LPS::calibratePosition(float iCal, float dCal, float jCal) {
Joseph_Penikis 0:dda98f02d71e 59 updateCalibrationDistances();
Joseph_Penikis 0:dda98f02d71e 60
Joseph_Penikis 0:dda98f02d71e 61 // Calculate the coordinates of beacon 1
Joseph_Penikis 0:dda98f02d71e 62 beacon_1_loc.x = calcX(beacon_2_loc.x, beacon_3_loc.x, dCal);
Joseph_Penikis 0:dda98f02d71e 63 beacon_1_loc.y = calcY(beacon_2_loc.x, beacon_1_distance, iCal, jCal, beacon_1_loc.x);
Joseph_Penikis 0:dda98f02d71e 64 beacon_1_loc.z = calcZ(beacon_2_loc.x, beacon_1_loc.x, beacon_1_loc.y);
Joseph_Penikis 0:dda98f02d71e 65
Joseph_Penikis 0:dda98f02d71e 66 // Push the data to new locations while becon 2 is calculated
Joseph_Penikis 0:dda98f02d71e 67 beacon_3_loc.x = beacon_2_loc.y; // 2_t1
Joseph_Penikis 0:dda98f02d71e 68 beacon_1_distance = beacon_2_loc.z; // 3_t1
Joseph_Penikis 0:dda98f02d71e 69
Joseph_Penikis 0:dda98f02d71e 70 beacon_2_loc.x = calcX(beacon_3_loc.x, beacon_3_loc.y, dCal);
Joseph_Penikis 0:dda98f02d71e 71 beacon_2_loc.y = calcY(beacon_3_loc.x, beacon_2_distance, iCal, jCal, beacon_2_loc.x);
Joseph_Penikis 0:dda98f02d71e 72 beacon_2_loc.z = calcZ(beacon_3_loc.x, beacon_2_loc.x, beacon_2_loc.y);
Joseph_Penikis 0:dda98f02d71e 73
Joseph_Penikis 0:dda98f02d71e 74 // Again, push data stored in beacon_3_loc out to safe location
Joseph_Penikis 0:dda98f02d71e 75 beacon_2_distance = beacon_3_loc.z; // 3_t2
Joseph_Penikis 0:dda98f02d71e 76
Joseph_Penikis 0:dda98f02d71e 77 beacon_3_loc.x = calcX(beacon_1_distance, beacon_2_distance, dCal);
Joseph_Penikis 0:dda98f02d71e 78 beacon_3_loc.y = calcY(beacon_1_distance, beacon_3_distance, iCal, jCal, beacon_3_loc.x);
Joseph_Penikis 0:dda98f02d71e 79 beacon_3_loc.z = calcZ(beacon_1_distance, beacon_3_loc.x, beacon_3_loc.y);
Joseph_Penikis 0:dda98f02d71e 80
Joseph_Penikis 0:dda98f02d71e 81 // All beacon locations should now have been acquired (Untested)
Joseph_Penikis 0:dda98f02d71e 82
Joseph_Penikis 0:dda98f02d71e 83 // Is it possible to calculate air speed and calibrate? TO VERIFY
Joseph_Penikis 0:dda98f02d71e 84
Joseph_Penikis 0:dda98f02d71e 85 // Now need to calculate unit vectors and translation values for i, j, and d
Joseph_Penikis 0:dda98f02d71e 86 // This is only required upon power on, and/or if beacons are moved
Joseph_Penikis 0:dda98f02d71e 87 calcUnitVectorsAndScalars();
Joseph_Penikis 0:dda98f02d71e 88 // Calibration complete!!!!!!!
Joseph_Penikis 0:dda98f02d71e 89 }
Joseph_Penikis 0:dda98f02d71e 90
Joseph_Penikis 1:b71a1c39d263 91 void LPS::updateCalDistances(bool isCalibrate) {
Joseph_Penikis 0:dda98f02d71e 92 // This will be the "socket" for talking to the base station
Joseph_Penikis 0:dda98f02d71e 93 // TODO: Implement...
Joseph_Penikis 0:dda98f02d71e 94
Joseph_Penikis 0:dda98f02d71e 95 // Use isCalibrate to handle interaction code to allow user to relocate to next calibration point
Joseph_Penikis 0:dda98f02d71e 96
Joseph_Penikis 0:dda98f02d71e 97 // Just dummy values for testing purposes
Joseph_Penikis 0:dda98f02d71e 98 beacon_1_distance = (11.55f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
Joseph_Penikis 0:dda98f02d71e 99 beacon_2_distance = (21.095f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
Joseph_Penikis 0:dda98f02d71e 100 beacon_3_distance = (15.395f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
Joseph_Penikis 0:dda98f02d71e 101 }
Joseph_Penikis 0:dda98f02d71e 102
Joseph_Penikis 1:b71a1c39d263 103 void LPS::updateDistances() {
Joseph_Penikis 0:dda98f02d71e 104 updateCalDistances(false);
Joseph_Penikis 0:dda98f02d71e 105 }
Joseph_Penikis 0:dda98f02d71e 106
Joseph_Penikis 1:b71a1c39d263 107 void LPS::updateCalibrationDistances() {
Joseph_Penikis 0:dda98f02d71e 108 // Reuse existing function to constrain external accessor code
Joseph_Penikis 0:dda98f02d71e 109 updateCalDistances(true);
Joseph_Penikis 0:dda98f02d71e 110
Joseph_Penikis 0:dda98f02d71e 111 beacon_2_loc.x = 14.14213562f;//beacon_1_distance; // 1_t1
Joseph_Penikis 0:dda98f02d71e 112 beacon_2_loc.y = 22.49444376f;//beacon_2_distance; // 1_t2
Joseph_Penikis 0:dda98f02d71e 113 beacon_2_loc.z = 12.36931688f;//beacon_3_distance; // 1_t3
Joseph_Penikis 0:dda98f02d71e 114
Joseph_Penikis 0:dda98f02d71e 115 updateCalDistances(true);
Joseph_Penikis 0:dda98f02d71e 116
Joseph_Penikis 0:dda98f02d71e 117 beacon_3_loc.x = 14.45683229f;//beacon_1_distance; // 2_t1
Joseph_Penikis 0:dda98f02d71e 118 beacon_3_loc.y = 21.47091055f;//beacon_2_distance; // 2_t2
Joseph_Penikis 0:dda98f02d71e 119 beacon_3_loc.z = 12.24744871f;//beacon_3_distance; // 2_t3
Joseph_Penikis 0:dda98f02d71e 120
Joseph_Penikis 0:dda98f02d71e 121 updateCalDistances(true);
Joseph_Penikis 0:dda98f02d71e 122
Joseph_Penikis 0:dda98f02d71e 123 beacon_1_distance = 16.673332f;
Joseph_Penikis 0:dda98f02d71e 124 beacon_2_distance = 22.36067477f;
Joseph_Penikis 0:dda98f02d71e 125 beacon_3_distance = 10.81665383f;
Joseph_Penikis 0:dda98f02d71e 126 // Third set is stored in the original defined variables, freeing up beacon_1_loc for initial calculation
Joseph_Penikis 0:dda98f02d71e 127 }
Joseph_Penikis 0:dda98f02d71e 128
Joseph_Penikis 2:21ca29888540 129 float calcX(float t1, float t2, float d) {
Joseph_Penikis 0:dda98f02d71e 130 /*
Joseph_Penikis 0:dda98f02d71e 131 x = (t1^2 - t2^2 + d^2) / 2d
Joseph_Penikis 0:dda98f02d71e 132 */
Joseph_Penikis 0:dda98f02d71e 133 return (t1 * t1 - t2 * t2 + d * d) / (2 * d);
Joseph_Penikis 0:dda98f02d71e 134 }
Joseph_Penikis 0:dda98f02d71e 135
Joseph_Penikis 2:21ca29888540 136 float calcY(float t1, float t3, float i, float j, float x) {
Joseph_Penikis 0:dda98f02d71e 137 /*
Joseph_Penikis 0:dda98f02d71e 138 y = (t1^2 - t3^2 +i^2 + j^2) / 2j - (i/j) * x
Joseph_Penikis 0:dda98f02d71e 139 */
Joseph_Penikis 0:dda98f02d71e 140 return ((pow(t1, 2) - pow(t3, 2) + pow(i, 2) + pow(j, 2)) / (2 * j)) - (i / j) * x;
Joseph_Penikis 0:dda98f02d71e 141 }
Joseph_Penikis 0:dda98f02d71e 142
Joseph_Penikis 2:21ca29888540 143 float calcZ(float t1, float x, float y) {
Joseph_Penikis 0:dda98f02d71e 144 /*
Joseph_Penikis 0:dda98f02d71e 145 z = sqrt(t1^2 - x^2 - y^2)
Joseph_Penikis 0:dda98f02d71e 146 */
Joseph_Penikis 0:dda98f02d71e 147 // Technically has two solutions both +/-, can I assume always positive? TODO: Handle inverse value if needed
Joseph_Penikis 0:dda98f02d71e 148 return sqrt(pow(t1, 2) - pow(x, 2) - pow(y, 2));
Joseph_Penikis 0:dda98f02d71e 149 }
Joseph_Penikis 0:dda98f02d71e 150
Joseph_Penikis 1:b71a1c39d263 151 void LPS::calcUnitVectorsAndScalars() {
Joseph_Penikis 0:dda98f02d71e 152 // e_x = P2 - P1 / |P2 - P1|
Joseph_Penikis 0:dda98f02d71e 153 _3D_Vector v = subTwoVectors(beacon_2_loc, beacon_1_loc);
Joseph_Penikis 0:dda98f02d71e 154
Joseph_Penikis 0:dda98f02d71e 155 unitX = unitVector(v);
Joseph_Penikis 0:dda98f02d71e 156
Joseph_Penikis 0:dda98f02d71e 157 // i = e_x dot P3 - P1
Joseph_Penikis 0:dda98f02d71e 158 v = subTwoVectors(beacon_3_loc, beacon_1_loc);
Joseph_Penikis 0:dda98f02d71e 159
Joseph_Penikis 0:dda98f02d71e 160 i = dot_Product(unitX, v);
Joseph_Penikis 0:dda98f02d71e 161
Joseph_Penikis 0:dda98f02d71e 162 // e_y = P3 - P1 - i.e_x / |P3 - P1 - i.e_x|
Joseph_Penikis 0:dda98f02d71e 163 v = subTwoVectors(v, scaleVector(unitX, i));
Joseph_Penikis 0:dda98f02d71e 164
Joseph_Penikis 0:dda98f02d71e 165 unitY = unitVector(v);
Joseph_Penikis 0:dda98f02d71e 166
Joseph_Penikis 0:dda98f02d71e 167 // e_z = e_x cross e_y
Joseph_Penikis 0:dda98f02d71e 168 unitZ = cross_Product(unitX, unitY);
Joseph_Penikis 0:dda98f02d71e 169
Joseph_Penikis 0:dda98f02d71e 170 // d = |P2 - P1|
Joseph_Penikis 0:dda98f02d71e 171 v = subTwoVectors(beacon_2_loc, beacon_1_loc);
Joseph_Penikis 0:dda98f02d71e 172
Joseph_Penikis 0:dda98f02d71e 173 d = vectorMagnitude(v);
Joseph_Penikis 0:dda98f02d71e 174
Joseph_Penikis 0:dda98f02d71e 175 // j = e_y dot P3 - P1
Joseph_Penikis 0:dda98f02d71e 176 v = subTwoVectors(beacon_3_loc, beacon_1_loc);
Joseph_Penikis 0:dda98f02d71e 177
Joseph_Penikis 0:dda98f02d71e 178 j = dot_Product(unitY, v);
Joseph_Penikis 0:dda98f02d71e 179 }
Joseph_Penikis 0:dda98f02d71e 180
Joseph_Penikis 1:b71a1c39d263 181 _3D_Vector LPS::getCurrentLocation() { return current_1; }
Joseph_Penikis 1:b71a1c39d263 182 _3D_Vector LPS::getBeacon_1_Location() { return beacon_1_loc; }
Joseph_Penikis 1:b71a1c39d263 183 _3D_Vector LPS::getBeacon_2_Location() { return beacon_2_loc; }
Joseph_Penikis 1:b71a1c39d263 184 _3D_Vector LPS::getBeacon_3_Location() { return beacon_3_loc; }
Joseph_Penikis 0:dda98f02d71e 185
Joseph_Penikis 0:dda98f02d71e 186 _3D_Vector addFourVectors(_3D_Vector a, _3D_Vector b, _3D_Vector c, _3D_Vector d) {
Joseph_Penikis 0:dda98f02d71e 187 _3D_Vector v;
Joseph_Penikis 0:dda98f02d71e 188
Joseph_Penikis 0:dda98f02d71e 189 v.x = a.x + b.x + c.x + d.x;
Joseph_Penikis 0:dda98f02d71e 190 v.y = a.y + b.y + c.y + d.y;
Joseph_Penikis 0:dda98f02d71e 191 v.z = a.z + b.z + c.z + d.z;
Joseph_Penikis 0:dda98f02d71e 192
Joseph_Penikis 0:dda98f02d71e 193 return v;
Joseph_Penikis 0:dda98f02d71e 194 }
Joseph_Penikis 0:dda98f02d71e 195
Joseph_Penikis 0:dda98f02d71e 196 _3D_Vector scaleVector(_3D_Vector a, float scale) {
Joseph_Penikis 0:dda98f02d71e 197 a.x *= scale;
Joseph_Penikis 0:dda98f02d71e 198 a.y *= scale;
Joseph_Penikis 0:dda98f02d71e 199 a.z *= scale;
Joseph_Penikis 0:dda98f02d71e 200
Joseph_Penikis 0:dda98f02d71e 201 return a;
Joseph_Penikis 0:dda98f02d71e 202 }
Joseph_Penikis 0:dda98f02d71e 203
Joseph_Penikis 0:dda98f02d71e 204 _3D_Vector unitVector(_3D_Vector a) {
Joseph_Penikis 0:dda98f02d71e 205 _3D_Vector v;
Joseph_Penikis 0:dda98f02d71e 206 float size = sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
Joseph_Penikis 0:dda98f02d71e 207
Joseph_Penikis 0:dda98f02d71e 208 v.x = a.x / size;
Joseph_Penikis 0:dda98f02d71e 209 v.y = a.y / size;
Joseph_Penikis 0:dda98f02d71e 210 v.z = a.z / size;
Joseph_Penikis 0:dda98f02d71e 211
Joseph_Penikis 0:dda98f02d71e 212 return v;
Joseph_Penikis 0:dda98f02d71e 213 }
Joseph_Penikis 0:dda98f02d71e 214
Joseph_Penikis 0:dda98f02d71e 215 float dot_Product(_3D_Vector a, _3D_Vector b) {
Joseph_Penikis 0:dda98f02d71e 216 return a.x * b.x + a.y * b.y + a.z * b.z;
Joseph_Penikis 0:dda98f02d71e 217 }
Joseph_Penikis 0:dda98f02d71e 218
Joseph_Penikis 0:dda98f02d71e 219 float vectorMagnitude(_3D_Vector a) {
Joseph_Penikis 0:dda98f02d71e 220 return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
Joseph_Penikis 0:dda98f02d71e 221 }
Joseph_Penikis 0:dda98f02d71e 222
Joseph_Penikis 0:dda98f02d71e 223 _3D_Vector cross_Product(_3D_Vector a, _3D_Vector b) {
Joseph_Penikis 0:dda98f02d71e 224 _3D_Vector v;
Joseph_Penikis 0:dda98f02d71e 225
Joseph_Penikis 0:dda98f02d71e 226 v.x = (a.y * b.z - a.z * b.y);
Joseph_Penikis 0:dda98f02d71e 227 v.y = (a.z * b.x - a.x * b.z);
Joseph_Penikis 0:dda98f02d71e 228 v.z = (a.x * b.y - a.y * b.x);
Joseph_Penikis 0:dda98f02d71e 229
Joseph_Penikis 0:dda98f02d71e 230 return v;
Joseph_Penikis 0:dda98f02d71e 231 }
Joseph_Penikis 0:dda98f02d71e 232
Joseph_Penikis 0:dda98f02d71e 233 _3D_Vector subTwoVectors(_3D_Vector a, _3D_Vector b) {
Joseph_Penikis 0:dda98f02d71e 234 _3D_Vector v;
Joseph_Penikis 0:dda98f02d71e 235
Joseph_Penikis 0:dda98f02d71e 236 v.x = a.x - b.x;
Joseph_Penikis 0:dda98f02d71e 237 v.y = a.y - b.y;
Joseph_Penikis 0:dda98f02d71e 238 v.z = a.z - b.z;
Joseph_Penikis 0:dda98f02d71e 239
Joseph_Penikis 0:dda98f02d71e 240 return v;
Joseph_Penikis 0:dda98f02d71e 241 }