Yelfie / LocalPositionSystem

Trilateration Based Local Position System

Dependents:   TDP_main_BartFork TDP_main TDP_main TDP_main_fork

LPS.h@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 #ifndef LPS_H
Joseph_Penikis 1:b71a1c39d263 2 #define LPS_H
Joseph_Penikis 1:b71a1c39d263 3
Joseph_Penikis 1:b71a1c39d263 4 #include "mbed.h"
Joseph_Penikis 1:b71a1c39d263 5
Joseph_Penikis 1:b71a1c39d263 6 typedef struct _3D_Vector{
Joseph_Penikis 1:b71a1c39d263 7 float x;
Joseph_Penikis 1:b71a1c39d263 8 float y;
Joseph_Penikis 1:b71a1c39d263 9 float z;
Joseph_Penikis 1:b71a1c39d263 10 } __3D_Vector;
Joseph_Penikis 1:b71a1c39d263 11
Joseph_Penikis 1:b71a1c39d263 12 class LPS {
Joseph_Penikis 1:b71a1c39d263 13 public:
Joseph_Penikis 1:b71a1c39d263 14 // Eventually use this to setup pin interconnects to Basestation
Joseph_Penikis 1:b71a1c39d263 15 LPS();
Joseph_Penikis 1:b71a1c39d263 16 ~LPS();
Joseph_Penikis 1:b71a1c39d263 17
Joseph_Penikis 1:b71a1c39d263 18 void calibratePosition(float iCal, float dCal, float jCal);
Joseph_Penikis 1:b71a1c39d263 19
Joseph_Penikis 1:b71a1c39d263 20 void setHome(_3D_Vector home);
Joseph_Penikis 1:b71a1c39d263 21
Joseph_Penikis 1:b71a1c39d263 22 void updateLocation();
Joseph_Penikis 1:b71a1c39d263 23
Joseph_Penikis 1:b71a1c39d263 24 float getCurrentX();
Joseph_Penikis 1:b71a1c39d263 25 float getCurrentY();
Joseph_Penikis 1:b71a1c39d263 26 float getCurrentZ();
Joseph_Penikis 1:b71a1c39d263 27 _3D_Vector getCurrentLocation();
Joseph_Penikis 1:b71a1c39d263 28
Joseph_Penikis 1:b71a1c39d263 29 _3D_Vector getBeacon_1_Location();
Joseph_Penikis 1:b71a1c39d263 30 _3D_Vector getBeacon_2_Location();
Joseph_Penikis 1:b71a1c39d263 31 _3D_Vector getBeacon_3_Location();
Joseph_Penikis 1:b71a1c39d263 32
Joseph_Penikis 1:b71a1c39d263 33 // debug functions
Joseph_Penikis 1:b71a1c39d263 34 _3D_Vector getUnitX();
Joseph_Penikis 1:b71a1c39d263 35 _3D_Vector getUnitY();
Joseph_Penikis 1:b71a1c39d263 36 _3D_Vector getUnitZ();
Joseph_Penikis 1:b71a1c39d263 37 float geti();
Joseph_Penikis 1:b71a1c39d263 38 float getd();
Joseph_Penikis 1:b71a1c39d263 39 float getj();
Joseph_Penikis 1:b71a1c39d263 40 private:
Joseph_Penikis 1:b71a1c39d263 41 /*
Joseph_Penikis 1:b71a1c39d263 42 The three displacemnet values corrosponding to the calibration positions
Joseph_Penikis 1:b71a1c39d263 43 - ALL MOST BE Z=0
Joseph_Penikis 1:b71a1c39d263 44 - ONE MUST BE ORIGIN (ALREADY ASSUMED)
Joseph_Penikis 1:b71a1c39d263 45 - ONE MUST BE ON X_AXIS
Joseph_Penikis 1:b71a1c39d263 46
Joseph_Penikis 1:b71a1c39d263 47 Cal Point 1 is origin (0,0,0)
Joseph_Penikis 1:b71a1c39d263 48 theta_1 : x-coordinate of Cal Point 2
Joseph_Penikis 1:b71a1c39d263 49 theta_2 : x-coordinate of Cal Point 3
Joseph_Penikis 1:b71a1c39d263 50 theta_3 : y-coordinate of Cal Point 3
Joseph_Penikis 1:b71a1c39d263 51
Joseph_Penikis 1:b71a1c39d263 52 i.e.
Joseph_Penikis 1:b71a1c39d263 53 Cal Point 1 : {0,0,0}
Joseph_Penikis 1:b71a1c39d263 54 Cal Point 2 : {theta_1,0,0}
Joseph_Penikis 1:b71a1c39d263 55 Cal Point 3 : {theta_2,theta_3,0}
Joseph_Penikis 1:b71a1c39d263 56 */
Joseph_Penikis 1:b71a1c39d263 57 // i,d and j
Joseph_Penikis 1:b71a1c39d263 58 float i,d,j;
Joseph_Penikis 1:b71a1c39d263 59 // Unit Vector
Joseph_Penikis 1:b71a1c39d263 60 _3D_Vector unitX, unitY, unitZ;
Joseph_Penikis 1:b71a1c39d263 61
Joseph_Penikis 1:b71a1c39d263 62 // The received distances from the beacons
Joseph_Penikis 1:b71a1c39d263 63 float beacon_1_distance;
Joseph_Penikis 1:b71a1c39d263 64 float beacon_2_distance;
Joseph_Penikis 1:b71a1c39d263 65 float beacon_3_distance;
Joseph_Penikis 1:b71a1c39d263 66
Joseph_Penikis 1:b71a1c39d263 67 // The computed location of all three beacons
Joseph_Penikis 1:b71a1c39d263 68 _3D_Vector beacon_1_loc;
Joseph_Penikis 1:b71a1c39d263 69 _3D_Vector beacon_2_loc;
Joseph_Penikis 1:b71a1c39d263 70 _3D_Vector beacon_3_loc;
Joseph_Penikis 1:b71a1c39d263 71
Joseph_Penikis 1:b71a1c39d263 72 // Home location
Joseph_Penikis 1:b71a1c39d263 73 _3D_Vector home;
Joseph_Penikis 1:b71a1c39d263 74 // Current location, two possible locations, still need to determine if i can narrow these down analytically,
Joseph_Penikis 1:b71a1c39d263 75 // currently, current_1 contains value closes to z = 0;
Joseph_Penikis 1:b71a1c39d263 76 _3D_Vector current_1;
Joseph_Penikis 1:b71a1c39d263 77 _3D_Vector current_2;
Joseph_Penikis 1:b71a1c39d263 78
Joseph_Penikis 1:b71a1c39d263 79 void updateCalDistances(bool isCalibrate);
Joseph_Penikis 1:b71a1c39d263 80 void updateDistances();
Joseph_Penikis 1:b71a1c39d263 81
Joseph_Penikis 1:b71a1c39d263 82 // Special function used for reading in and storing all 9 distances needed for calibration, abuse _3D_Vector to optimise memory
Joseph_Penikis 1:b71a1c39d263 83 void updateCalibrationDistances();
Joseph_Penikis 1:b71a1c39d263 84
Joseph_Penikis 1:b71a1c39d263 85 // Calculate all the necessary unitX, unitY, unitZ, i, d, and j values
Joseph_Penikis 1:b71a1c39d263 86 void calcUnitVectorsAndScalars();
Joseph_Penikis 1:b71a1c39d263 87
Joseph_Penikis 1:b71a1c39d263 88 };
Joseph_Penikis 1:b71a1c39d263 89
Joseph_Penikis 2:21ca29888540 90
Joseph_Penikis 2:21ca29888540 91 // Calculates the coordinates based off of received timings/Calibration mode, values are dependent on mode (Calibrate or Acquire)
Joseph_Penikis 2:21ca29888540 92 // Passing by reference just saves a bit of memery (and time?)
Joseph_Penikis 2:21ca29888540 93 float calcX(float t1, float t2, float d);
Joseph_Penikis 2:21ca29888540 94 float calcY(float t1, float t3, float i, float j, float x);
Joseph_Penikis 2:21ca29888540 95 float calcZ(float t1, float x, float y);
Joseph_Penikis 2:21ca29888540 96
Joseph_Penikis 1:b71a1c39d263 97 // Static functions that don't belong to the object
Joseph_Penikis 1:b71a1c39d263 98 _3D_Vector addFourVectors(_3D_Vector a, _3D_Vector b, _3D_Vector c, _3D_Vector d);
Joseph_Penikis 1:b71a1c39d263 99 _3D_Vector scaleVector(_3D_Vector a, float scale);
Joseph_Penikis 1:b71a1c39d263 100 _3D_Vector unitVector(_3D_Vector a);
Joseph_Penikis 1:b71a1c39d263 101
Joseph_Penikis 1:b71a1c39d263 102 float dot_Product(_3D_Vector a, _3D_Vector b);
Joseph_Penikis 1:b71a1c39d263 103 float vectorMagnitude(_3D_Vector a);
Joseph_Penikis 1:b71a1c39d263 104 _3D_Vector cross_Product(_3D_Vector a, _3D_Vector b);
Joseph_Penikis 1:b71a1c39d263 105 _3D_Vector subTwoVectors(_3D_Vector a, _3D_Vector b);
Joseph_Penikis 1:b71a1c39d263 106 #endif