File content as of revision 3:dd68ac680416:

#include "LPS.h"

// Defines the delay experienced between transmitting IR and Ultrasonic pulse
#define PING_OFFSET 0
#define SPEED_OF_SOUND 330.f
// Used to rectify the issue of measured spheres not intersecting, may not be needed, but available anyway
#define SWELL_VALUE 0

// Defines for instructions on SPI link, instructions corrpsond to distances under ~20mm, way too close to ever be picked up...
#define START_CAL 0xFF
#define REQUEST_T1 0xFE
#define REQUEST_T2 0xFD 
#define REQUEST_T3 0xFC
#define NEXT_CAL_POINT 0xFB
#define UPPER_BYTE 0x01

LPS::LPS(PinName MOSI, PinName MISO, PinName SCLK) : _spi(MOSI, MISO, SCLK) {
    // Set up SPI interface for AVR
    _spi.format(8,3);
    _spi.frequency(1000000);
}
LPS::~LPS() {}

_3D_Vector LPS::getUnitX() { return unitX; }
_3D_Vector LPS::getUnitY() { return unitY; }
_3D_Vector LPS::getUnitZ() { return unitZ; }
float LPS::getd() { return d; }
float LPS::geti() { return i; }
float LPS::getj() { return j; }

void LPS::updateLocation() {
    updateDistances();
    
    current_1.x = calcX(beacon_1_distance, beacon_2_distance, d);
    current_1.y = calcY(beacon_1_distance, beacon_3_distance, i, j, current_1.x);
    current_1.z = calcZ(beacon_1_distance, current_1.x, current_1.y);
    
    _3D_Vector scaledX = scaleVector(unitX, current_1.x);
    _3D_Vector scaledY = scaleVector(unitY, current_1.y);
    _3D_Vector scaledZ = scaleVector(unitZ, current_1.z);
    
    current_1.x = beacon_1_loc.x + scaledX.x + scaledY.x;
    current_1.y = beacon_1_loc.y + scaledX.y + scaledY.y;
    current_1.z = beacon_1_loc.z + scaledX.z + scaledY.z;
    
    current_2.x = current_1.x - scaledZ.x;
    current_2.y = current_1.y - scaledZ.y;
    current_2.z = current_1.z - scaledZ.z;
    
    current_1.x += scaledZ.x;
    current_1.y += scaledZ.y;
    current_1.z += scaledZ.z;
    
    // Set location closest to z=0 as location, (We are typically on the floor, but need to verify this solution works)
    // If current_2.z is smaller than current_1.z, swap them
    if (abs(current_1.z) > abs(current_2.z)) {
        _3D_Vector temp = current_1;
        current_1.x = current_2.x;
        current_1.y = current_2.y;
        current_1.z = current_2.z;
        
        current_2.x = temp.x;
        current_2.y = temp.y;
        current_2.z = temp.z;
    }
    
    // Both solutions are currently preserved incase of neccisity in future use
}

void LPS::calibratePosition(float iCal, float dCal, float jCal) {    
    updateCalibrationDistances();
    
    // Calculate the coordinates of beacon 1
    beacon_1_loc.x = calcX(beacon_2_loc.x, beacon_3_loc.x, dCal);
    beacon_1_loc.y = calcY(beacon_2_loc.x, beacon_1_distance, iCal, jCal, beacon_1_loc.x);
    beacon_1_loc.z = calcZ(beacon_2_loc.x, beacon_1_loc.x, beacon_1_loc.y);
    
    // Push the data to new locations while becon 2 is calculated
    beacon_3_loc.x = beacon_2_loc.y;            // 2_t1
    beacon_1_distance = beacon_2_loc.z;         // 3_t1                             
    
    beacon_2_loc.x = calcX(beacon_3_loc.x, beacon_3_loc.y, dCal);
    beacon_2_loc.y = calcY(beacon_3_loc.x, beacon_2_distance, iCal, jCal, beacon_2_loc.x);
    beacon_2_loc.z = calcZ(beacon_3_loc.x, beacon_2_loc.x, beacon_2_loc.y);
    
    // Again, push data stored in beacon_3_loc out to safe location
    beacon_2_distance = beacon_3_loc.z;         // 3_t2
    
    beacon_3_loc.x = calcX(beacon_1_distance, beacon_2_distance, dCal);
    beacon_3_loc.y = calcY(beacon_1_distance, beacon_3_distance, iCal, jCal, beacon_3_loc.x);
    beacon_3_loc.z = calcZ(beacon_1_distance, beacon_3_loc.x, beacon_3_loc.y);
    
    // All beacon locations should now have been acquired (Untested)
    
    // Is it possible to calculate air speed and calibrate?  TO VERIFY
    
    // Now need to calculate unit vectors and translation values for i, j, and d
    // This is only required upon power on, and/or if beacons are moved
    calcUnitVectorsAndScalars();
    // Calibration complete!!!!!!!
}

int LPS::fetchTimeOverSPI(int instr) {
     
    // 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
    do {
        received_2 = _spi.write(instr + UPPER_BYTE);    
    } while (received_2 & 0x80);
    
    // I have received some data back, the avr is setup to create an identical packet to check data is correct
    // Send this data back to AVR (Not really used here, but better than using defined instructions)
    do {
        // 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
        received_1 = received_2;
        received_2 = _spi.write(received_1);
    } while (received_1 != received_2);
    // Valid data got (UPPER BYTE)
    
    do {
        received_3 = _spi.write(instr);    
    } while (received_3 == instr); 
    do {
        // 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
        received_1 = received_3;
        received_3 = _spi.write(received_1);
    } while (received_1 != received_3);
    // Valid data got (LOWER BYTE)
    
    // Generate an integer from these and return
    return (received_2 << 8) + received_3;
}

void LPS::updateDistances() {
    // This will be the "socket" for talking to the base station    
    /*
    beacon_1_distance = (fetchTimeOverSPI(REQUEST_T1) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
    beacon_2_distance = (fetchTimeOverSPI(REQUEST_T2) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
    beacon_3_distance = (fetchTimeOverSPI(REQUEST_T3) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
    */
    
    // Just dummy values for testing purposes
    beacon_1_distance = 11.55f;
    beacon_2_distance = 21.095f;
    beacon_3_distance = 15.395f;  
}

void LPS::updateCalibrationDistances() {
    // Tell base station to enter calibration mode
    _spi.write(START_CAL);   
    
    // Reuse existing function to constrain external accessor code
    updateDistances();  
    
    beacon_2_loc.x = 14.14213562f;//beacon_1_distance;     // 1_t1
    beacon_2_loc.y = 22.49444376f;//beacon_2_distance;     // 1_t2
    beacon_2_loc.z = 12.36931688f;//beacon_3_distance;     // 1_t3
    
    updateDistances(); 
    
    beacon_3_loc.x = 14.45683229f;//beacon_1_distance;     // 2_t1
    beacon_3_loc.y = 21.47091055f;//beacon_2_distance;     // 2_t2
    beacon_3_loc.z = 12.24744871f;//beacon_3_distance;     // 2_t3
    
    updateDistances();
    
    beacon_1_distance = 16.673332f;
    beacon_2_distance = 22.36067477f;
    beacon_3_distance = 10.81665383f;
    // Third set is stored in the original defined variables, freeing up beacon_1_loc for initial calculation
}

float calcX(float t1, float t2, float d) {  
    /*
        x = (t1^2 - t2^2 + d^2) / 2d
    */  
    return (t1 * t1 - t2 * t2 + d * d) / (2 * d);
}

float calcY(float t1, float t3, float i, float j, float x) {
    /*
        y = (t1^2 - t3^2 +i^2 + j^2) / 2j   -   (i/j) * x
    */
    return ((pow(t1, 2) - pow(t3, 2) + pow(i, 2) + pow(j, 2)) / (2 * j)) - (i / j) * x;
}

float calcZ(float t1, float x, float y) {
    /*
        z = sqrt(t1^2 - x^2 - y^2)
    */
    // Technically has two solutions both +/-, can I assume always positive? TODO: Handle inverse value if needed
    return sqrt(pow(t1, 2) - pow(x, 2) - pow(y, 2));   
}

void LPS::calcUnitVectorsAndScalars() {
     // e_x = P2 - P1 / |P2 - P1|
     _3D_Vector v = subTwoVectors(beacon_2_loc, beacon_1_loc);
     
     unitX = unitVector(v);
     
     // i = e_x dot P3 - P1
     v = subTwoVectors(beacon_3_loc, beacon_1_loc);
     
     i = dot_Product(unitX, v);
     
     // e_y = P3 - P1 - i.e_x / |P3 - P1 - i.e_x|     
     v = subTwoVectors(v, scaleVector(unitX, i));
     
     unitY = unitVector(v);
     
     // e_z = e_x cross e_y
     unitZ = cross_Product(unitX, unitY);
     
     // d = |P2 - P1|
     v = subTwoVectors(beacon_2_loc, beacon_1_loc);
     
     d = vectorMagnitude(v);
     
     // j = e_y dot P3 - P1
     v = subTwoVectors(beacon_3_loc, beacon_1_loc);
     
     j = dot_Product(unitY, v);  
}

_3D_Vector LPS::getCurrentLocation() { return current_1; }  
_3D_Vector LPS::getBeacon_1_Location() { return beacon_1_loc; }
_3D_Vector LPS::getBeacon_2_Location() { return beacon_2_loc; }
_3D_Vector LPS::getBeacon_3_Location() { return beacon_3_loc; }

_3D_Vector addFourVectors(_3D_Vector a, _3D_Vector b, _3D_Vector c, _3D_Vector d) {
    _3D_Vector v;
    
    v.x = a.x + b.x + c.x + d.x;
    v.y = a.y + b.y + c.y + d.y;
    v.z = a.z + b.z + c.z + d.z;
    
    return v;
}

_3D_Vector scaleVector(_3D_Vector a, float scale) {    
    a.x *= scale;
    a.y *= scale;
    a.z *= scale;
    
    return a;
}

_3D_Vector unitVector(_3D_Vector a) {
    _3D_Vector v;
    float size = sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
    
    v.x = a.x / size;
    v.y = a.y / size;
    v.z = a.z / size;
    
    return v;
}

float dot_Product(_3D_Vector a, _3D_Vector b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

float vectorMagnitude(_3D_Vector a) {
    return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);   
}

_3D_Vector cross_Product(_3D_Vector a, _3D_Vector b) {
    _3D_Vector v;
    
    v.x = (a.y * b.z - a.z * b.y);
    v.y = (a.z * b.x - a.x * b.z);
    v.z = (a.x * b.y - a.y * b.x);
    
    return v;
}
  
_3D_Vector subTwoVectors(_3D_Vector a, _3D_Vector b) {
    _3D_Vector v;
    
    v.x = a.x - b.x;
    v.y = a.y - b.y;
    v.z = a.z - b.z;
    
    return v;
}