LPS.cpp

00001 #include "LPS.h"
00002 
00003 // Defines the delay experienced between transmitting IR and Ultrasonic pulse
00004 #define PING_OFFSET 0
00005 #define SPEED_OF_SOUND 330.f
00006 // Used to rectify the issue of measured spheres not intersecting, may not be needed, but available anyway
00007 #define SWELL_VALUE 0
00008 
00009 // Defines for instructions on SPI link, instructions corrpsond to distances under ~20mm, way too close to ever be picked up...
00010 #define START_CAL 0xFF
00011 #define REQUEST_T1 0xFE
00012 #define REQUEST_T2 0xFD 
00013 #define REQUEST_T3 0xFC
00014 #define NEXT_CAL_POINT 0xFB
00015 #define UPPER_BYTE 0x01
00016 
00017 LPS::LPS(PinName MOSI, PinName MISO, PinName SCLK) : _spi(MOSI, MISO, SCLK) {
00018     // Set up SPI interface for AVR
00019     _spi.format(8,3);
00020     _spi.frequency(1000000);
00021 }
00022 LPS::~LPS() {}
00023 
00024 _3D_Vector LPS::getUnitX() { return unitX; }
00025 _3D_Vector LPS::getUnitY() { return unitY; }
00026 _3D_Vector LPS::getUnitZ() { return unitZ; }
00027 float LPS::getd() { return d; }
00028 float LPS::geti() { return i; }
00029 float LPS::getj() { return j; }
00030 
00031 void LPS::updateLocation() {
00032     updateDistances();
00033     
00034     current_1.x = calcX(beacon_1_distance, beacon_2_distance, d);
00035     current_1.y = calcY(beacon_1_distance, beacon_3_distance, i, j, current_1.x);
00036     current_1.z = calcZ(beacon_1_distance, current_1.x, current_1.y);
00037     
00038     _3D_Vector scaledX = scaleVector(unitX, current_1.x);
00039     _3D_Vector scaledY = scaleVector(unitY, current_1.y);
00040     _3D_Vector scaledZ = scaleVector(unitZ, current_1.z);
00041     
00042     current_1.x = beacon_1_loc.x + scaledX.x + scaledY.x;
00043     current_1.y = beacon_1_loc.y + scaledX.y + scaledY.y;
00044     current_1.z = beacon_1_loc.z + scaledX.z + scaledY.z;
00045     
00046     current_2.x = current_1.x - scaledZ.x;
00047     current_2.y = current_1.y - scaledZ.y;
00048     current_2.z = current_1.z - scaledZ.z;
00049     
00050     current_1.x += scaledZ.x;
00051     current_1.y += scaledZ.y;
00052     current_1.z += scaledZ.z;
00053     
00054     // Set location closest to z=0 as location, (We are typically on the floor, but need to verify this solution works)
00055     // If current_2.z is smaller than current_1.z, swap them
00056     if (abs(current_1.z) > abs(current_2.z)) {
00057         _3D_Vector temp = current_1;
00058         current_1.x = current_2.x;
00059         current_1.y = current_2.y;
00060         current_1.z = current_2.z;
00061         
00062         current_2.x = temp.x;
00063         current_2.y = temp.y;
00064         current_2.z = temp.z;
00065     }
00066     
00067     // Both solutions are currently preserved incase of neccisity in future use
00068 }
00069 
00070 void LPS::calibratePosition(float iCal, float dCal, float jCal) {    
00071     updateCalibrationDistances();
00072     
00073     // Calculate the coordinates of beacon 1
00074     beacon_1_loc.x = calcX(beacon_2_loc.x, beacon_3_loc.x, dCal);
00075     beacon_1_loc.y = calcY(beacon_2_loc.x, beacon_1_distance, iCal, jCal, beacon_1_loc.x);
00076     beacon_1_loc.z = calcZ(beacon_2_loc.x, beacon_1_loc.x, beacon_1_loc.y);
00077     
00078     // Push the data to new locations while becon 2 is calculated
00079     beacon_3_loc.x = beacon_2_loc.y;            // 2_t1
00080     beacon_1_distance = beacon_2_loc.z;         // 3_t1                             
00081     
00082     beacon_2_loc.x = calcX(beacon_3_loc.x, beacon_3_loc.y, dCal);
00083     beacon_2_loc.y = calcY(beacon_3_loc.x, beacon_2_distance, iCal, jCal, beacon_2_loc.x);
00084     beacon_2_loc.z = calcZ(beacon_3_loc.x, beacon_2_loc.x, beacon_2_loc.y);
00085     
00086     // Again, push data stored in beacon_3_loc out to safe location
00087     beacon_2_distance = beacon_3_loc.z;         // 3_t2
00088     
00089     beacon_3_loc.x = calcX(beacon_1_distance, beacon_2_distance, dCal);
00090     beacon_3_loc.y = calcY(beacon_1_distance, beacon_3_distance, iCal, jCal, beacon_3_loc.x);
00091     beacon_3_loc.z = calcZ(beacon_1_distance, beacon_3_loc.x, beacon_3_loc.y);
00092     
00093     // All beacon locations should now have been acquired (Untested)
00094     
00095     // Is it possible to calculate air speed and calibrate?  TO VERIFY
00096     
00097     // Now need to calculate unit vectors and translation values for i, j, and d
00098     // This is only required upon power on, and/or if beacons are moved
00099     calcUnitVectorsAndScalars();
00100     // Calibration complete!!!!!!!
00101 }
00102 
00103 int LPS::fetchTimeOverSPI(int instr) {
00104      
00105     // Loop while avr has not prepared data, i.e. still returning an instruction not data. TODO: Make a fetch only if available option to save on processor time
00106     do {
00107         received_2 = _spi.write(instr + UPPER_BYTE);    
00108     } while (received_2 & 0x80);
00109     
00110     // I have received some data back, the avr is setup to create an identical packet to check data is correct
00111     // Send this data back to AVR (Not really used here, but better than using defined instructions)
00112     do {
00113         // Each time this loop data_1 shifted to r_1, data_2 loaded into r_2, i.e. r_2 ALWAYS has the most recent value
00114         received_1 = received_2;
00115         received_2 = _spi.write(received_1);
00116     } while (received_1 != received_2);
00117     // Valid data got (UPPER BYTE)
00118     
00119     do {
00120         received_3 = _spi.write(instr);    
00121     } while (received_3 == instr); 
00122     do {
00123         // Each time this loop data_1 shifted to r_1, data_2 loaded into r_3, i.e. r_3 ALWAYS has the most recent value
00124         received_1 = received_3;
00125         received_3 = _spi.write(received_1);
00126     } while (received_1 != received_3);
00127     // Valid data got (LOWER BYTE)
00128     
00129     // Generate an integer from these and return
00130     return (received_2 << 8) + received_3;
00131 }
00132 
00133 void LPS::updateDistances() {
00134     // This will be the "socket" for talking to the base station    
00135     /*
00136     beacon_1_distance = (fetchTimeOverSPI(REQUEST_T1) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
00137     beacon_2_distance = (fetchTimeOverSPI(REQUEST_T2) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
00138     beacon_3_distance = (fetchTimeOverSPI(REQUEST_T3) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
00139     */
00140     
00141     // Just dummy values for testing purposes
00142     beacon_1_distance = 11.55f;
00143     beacon_2_distance = 21.095f;
00144     beacon_3_distance = 15.395f;  
00145 }
00146 
00147 void LPS::updateCalibrationDistances() {
00148     // Tell base station to enter calibration mode
00149     _spi.write(START_CAL);   
00150     
00151     // Reuse existing function to constrain external accessor code
00152     updateDistances();  
00153     
00154     beacon_2_loc.x = 14.14213562f;//beacon_1_distance;     // 1_t1
00155     beacon_2_loc.y = 22.49444376f;//beacon_2_distance;     // 1_t2
00156     beacon_2_loc.z = 12.36931688f;//beacon_3_distance;     // 1_t3
00157     
00158     updateDistances(); 
00159     
00160     beacon_3_loc.x = 14.45683229f;//beacon_1_distance;     // 2_t1
00161     beacon_3_loc.y = 21.47091055f;//beacon_2_distance;     // 2_t2
00162     beacon_3_loc.z = 12.24744871f;//beacon_3_distance;     // 2_t3
00163     
00164     updateDistances();
00165     
00166     beacon_1_distance = 16.673332f;
00167     beacon_2_distance = 22.36067477f;
00168     beacon_3_distance = 10.81665383f;
00169     // Third set is stored in the original defined variables, freeing up beacon_1_loc for initial calculation
00170 }
00171 
00172 float calcX(float t1, float t2, float d) {  
00173     /*
00174         x = (t1^2 - t2^2 + d^2) / 2d
00175     */  
00176     return (t1 * t1 - t2 * t2 + d * d) / (2 * d);
00177 }
00178 
00179 float calcY(float t1, float t3, float i, float j, float x) {
00180     /*
00181         y = (t1^2 - t3^2 +i^2 + j^2) / 2j   -   (i/j) * x
00182     */
00183     return ((pow(t1, 2) - pow(t3, 2) + pow(i, 2) + pow(j, 2)) / (2 * j)) - (i / j) * x;
00184 }
00185 
00186 float calcZ(float t1, float x, float y) {
00187     /*
00188         z = sqrt(t1^2 - x^2 - y^2)
00189     */
00190     // Technically has two solutions both +/-, can I assume always positive? TODO: Handle inverse value if needed
00191     return sqrt(pow(t1, 2) - pow(x, 2) - pow(y, 2));   
00192 }
00193 
00194 void LPS::calcUnitVectorsAndScalars() {
00195      // e_x = P2 - P1 / |P2 - P1|
00196      _3D_Vector v = subTwoVectors(beacon_2_loc, beacon_1_loc);
00197      
00198      unitX = unitVector(v);
00199      
00200      // i = e_x dot P3 - P1
00201      v = subTwoVectors(beacon_3_loc, beacon_1_loc);
00202      
00203      i = dot_Product(unitX, v);
00204      
00205      // e_y = P3 - P1 - i.e_x / |P3 - P1 - i.e_x|     
00206      v = subTwoVectors(v, scaleVector(unitX, i));
00207      
00208      unitY = unitVector(v);
00209      
00210      // e_z = e_x cross e_y
00211      unitZ = cross_Product(unitX, unitY);
00212      
00213      // d = |P2 - P1|
00214      v = subTwoVectors(beacon_2_loc, beacon_1_loc);
00215      
00216      d = vectorMagnitude(v);
00217      
00218      // j = e_y dot P3 - P1
00219      v = subTwoVectors(beacon_3_loc, beacon_1_loc);
00220      
00221      j = dot_Product(unitY, v);  
00222 }
00223 
00224 _3D_Vector LPS::getCurrentLocation() { return current_1; }  
00225 _3D_Vector LPS::getBeacon_1_Location() { return beacon_1_loc; }
00226 _3D_Vector LPS::getBeacon_2_Location() { return beacon_2_loc; }
00227 _3D_Vector LPS::getBeacon_3_Location() { return beacon_3_loc; }
00228 
00229 _3D_Vector addFourVectors(_3D_Vector a, _3D_Vector b, _3D_Vector c, _3D_Vector d) {
00230     _3D_Vector v;
00231     
00232     v.x = a.x + b.x + c.x + d.x;
00233     v.y = a.y + b.y + c.y + d.y;
00234     v.z = a.z + b.z + c.z + d.z;
00235     
00236     return v;
00237 }
00238 
00239 _3D_Vector scaleVector(_3D_Vector a, float scale) {    
00240     a.x *= scale;
00241     a.y *= scale;
00242     a.z *= scale;
00243     
00244     return a;
00245 }
00246 
00247 _3D_Vector unitVector(_3D_Vector a) {
00248     _3D_Vector v;
00249     float size = sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
00250     
00251     v.x = a.x / size;
00252     v.y = a.y / size;
00253     v.z = a.z / size;
00254     
00255     return v;
00256 }
00257 
00258 float dot_Product(_3D_Vector a, _3D_Vector b) {
00259     return a.x * b.x + a.y * b.y + a.z * b.z;
00260 }
00261 
00262 float vectorMagnitude(_3D_Vector a) {
00263     return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);   
00264 }
00265 
00266 _3D_Vector cross_Product(_3D_Vector a, _3D_Vector b) {
00267     _3D_Vector v;
00268     
00269     v.x = (a.y * b.z - a.z * b.y);
00270     v.y = (a.z * b.x - a.x * b.z);
00271     v.z = (a.x * b.y - a.y * b.x);
00272     
00273     return v;
00274 }
00275   
00276 _3D_Vector subTwoVectors(_3D_Vector a, _3D_Vector b) {
00277     _3D_Vector v;
00278     
00279     v.x = a.x - b.x;
00280     v.y = a.y - b.y;
00281     v.z = a.z - b.z;
00282     
00283     return v;
00284 }