Revision 2:be5fd562cbd4, committed 2016-07-25

Comitter:

srago001

Date:

Mon Jul 25 04:02:32 2016 +0000

Parent:

1:6126bf6cdfae

Commit message:

Sonya's Pressure Sensor Addition

Changed in this revision

diff -r 6126bf6cdfae -r be5fd562cbd4 sensor.h
--- a/sensor.h	Mon Jul 25 00:19:24 2016 +0000
+++ b/sensor.h	Mon Jul 25 04:02:32 2016 +0000
@@ -1,15 +1,37 @@
 #include "MPU6050.h"
 #include "MS5803.h"
-#include "MS5837.h"
+//#include "MS5837.h"
 #include "IMU.h"
 #include "HMC5883L.h"
 
+#include <string>
+#include <vector>
+
+Serial device(PA_9,PA_10); // pressure sensor
 MPU6050 mpu6050;
 HMC5883L compass(I2C_SDA, I2C_SCL);
-float depth, heading;
+float depth = 0.0, heading;
 int16_t mag[3] = {0};
 
 
+void callback(){                // pressure sensor interrupt
+    char a = 0;
+    char i = 0;
+    char buffer[10] = {' '};
+    if (device.readable()) {
+        while(a != 'd') {
+            a = device.getc();
+            if ((a >= '0' && a <='9') || a == '.'){
+                    buffer[i] = a;
+                    i++;
+            }
+        }
+        depth = atof(buffer);
+        //pc.printf("Depth: '%f'\n", depth); 
+    }
+    
+} 
+
 void sensor_init() {
     IMUinit(mpu6050);
     wait_ms(100);
@@ -26,9 +48,15 @@
     //gets heading
     compass.getXYZ(mag);
     
+    float mx,my,mz;
+    mx = mag[1];
+    my = -1*mag[0];
+    mz = mag[2];
+    
+    
     float xh, yh;
-    xh = mag[0]*cos(pitch*PI/180) + mag[1]*sin(roll*PI/180)*sin(pitch*PI/180) - mag[2]*cos(roll*PI/180)*sin(pitch*PI/180);
-    yh = mag[1]*cos(roll*PI/180) + mag[2]*sin(roll*PI/180);
+    xh = mx*cos(pitch*PI/180) + my*sin(roll*PI/180)*sin(pitch*PI/180) - mz*cos(roll*PI/180)*sin(pitch*PI/180);
+    yh = my*cos(roll*PI/180) + mz*sin(roll*PI/180);
     heading = atan2(yh, xh) * 180/PI;
 }