Diff: LPS.cpp

Revision:

3:dd68ac680416

Parent:

2:21ca29888540

--- a/LPS.cpp	Wed Mar 11 01:20:56 2015 +0000
+++ b/LPS.cpp	Thu Mar 12 22:12:18 2015 +0000
@@ -2,11 +2,23 @@
 
 // Defines the delay experienced between transmitting IR and Ultrasonic pulse
 #define PING_OFFSET 0
-#define SPEED_OF_SOUND 1.0f
+#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
 
-LPS::LPS() {}
+// 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; }
@@ -88,37 +100,68 @@
     // Calibration complete!!!!!!!
 }
 
-void LPS::updateCalDistances(bool isCalibrate) {
-    // This will be the "socket" for talking to the base station
-    // TODO: Implement...
+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);
     
-    // Use isCalibrate to handle interaction code to allow user to relocate to next calibration point
+    // 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)
     
-    // Just dummy values for testing purposes
-    beacon_1_distance = (11.55f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
-    beacon_2_distance = (21.095f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
-    beacon_3_distance = (15.395f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;  
+    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() {
-    updateCalDistances(false);
+    // 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
-    updateCalDistances(true);  
+    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
     
-    updateCalDistances(true); 
+    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
     
-    updateCalDistances(true);
+    updateDistances();
     
     beacon_1_distance = 16.673332f;
     beacon_2_distance = 22.36067477f;