Sonya Ragothaman / sensor

Sonya's revisions to sensor.h

Fork of sensor by Andrew Olguin

sensor.h@2:be5fd562cbd4, 2016-07-25 (annotated)

Committer:
srago001
Date:
Mon Jul 25 04:02:32 2016 +0000
Revision:
2:be5fd562cbd4
Parent:
1:6126bf6cdfae 
Sonya's Pressure Sensor Addition

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aolgu003 0:13ac527b4c4b 1 #include "MPU6050.h"
aolgu003 0:13ac527b4c4b 2 #include "MS5803.h"
srago001 2:be5fd562cbd4 3 //#include "MS5837.h"
aolgu003 0:13ac527b4c4b 4 #include "IMU.h"
aolgu003 0:13ac527b4c4b 5 #include "HMC5883L.h"
aolgu003 0:13ac527b4c4b 6
srago001 2:be5fd562cbd4 7 #include <string>
srago001 2:be5fd562cbd4 8 #include <vector>
srago001 2:be5fd562cbd4 9
srago001 2:be5fd562cbd4 10 Serial device(PA_9,PA_10); // pressure sensor
aolgu003 0:13ac527b4c4b 11 MPU6050 mpu6050;
aolgu003 1:6126bf6cdfae 12 HMC5883L compass(I2C_SDA, I2C_SCL);
srago001 2:be5fd562cbd4 13 float depth = 0.0, heading;
aolgu003 0:13ac527b4c4b 14 int16_t mag[3] = {0};
aolgu003 0:13ac527b4c4b 15
aolgu003 0:13ac527b4c4b 16
srago001 2:be5fd562cbd4 17 void callback(){ // pressure sensor interrupt
srago001 2:be5fd562cbd4 18 char a = 0;
srago001 2:be5fd562cbd4 19 char i = 0;
srago001 2:be5fd562cbd4 20 char buffer[10] = {' '};
srago001 2:be5fd562cbd4 21 if (device.readable()) {
srago001 2:be5fd562cbd4 22 while(a != 'd') {
srago001 2:be5fd562cbd4 23 a = device.getc();
srago001 2:be5fd562cbd4 24 if ((a >= '0' && a <='9') || a == '.'){
srago001 2:be5fd562cbd4 25 buffer[i] = a;
srago001 2:be5fd562cbd4 26 i++;
srago001 2:be5fd562cbd4 27 }
srago001 2:be5fd562cbd4 28 }
srago001 2:be5fd562cbd4 29 depth = atof(buffer);
srago001 2:be5fd562cbd4 30 //pc.printf("Depth: '%f'\n", depth);
srago001 2:be5fd562cbd4 31 }
srago001 2:be5fd562cbd4 32
srago001 2:be5fd562cbd4 33 }
srago001 2:be5fd562cbd4 34
aolgu003 0:13ac527b4c4b 35 void sensor_init() {
aolgu003 0:13ac527b4c4b 36 IMUinit(mpu6050);
aolgu003 0:13ac527b4c4b 37 wait_ms(100);
aolgu003 0:13ac527b4c4b 38
aolgu003 0:13ac527b4c4b 39 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 40 pc.printf("Initialized begining readings\n");
aolgu003 0:13ac527b4c4b 41 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 42 }
aolgu003 0:13ac527b4c4b 43
aolgu003 0:13ac527b4c4b 44 void sensor_update() {
aolgu003 0:13ac527b4c4b 45 //IMU Update gets pitch and roll
aolgu003 0:13ac527b4c4b 46 IMUUpdate(mpu6050);
aolgu003 0:13ac527b4c4b 47
aolgu003 0:13ac527b4c4b 48 //gets heading
aolgu003 0:13ac527b4c4b 49 compass.getXYZ(mag);
aolgu003 0:13ac527b4c4b 50
srago001 2:be5fd562cbd4 51 float mx,my,mz;
srago001 2:be5fd562cbd4 52 mx = mag[1];
srago001 2:be5fd562cbd4 53 my = -1*mag[0];
srago001 2:be5fd562cbd4 54 mz = mag[2];
srago001 2:be5fd562cbd4 55
srago001 2:be5fd562cbd4 56
aolgu003 0:13ac527b4c4b 57 float xh, yh;
srago001 2:be5fd562cbd4 58 xh = mx*cos(pitch*PI/180) + my*sin(roll*PI/180)*sin(pitch*PI/180) - mz*cos(roll*PI/180)*sin(pitch*PI/180);
srago001 2:be5fd562cbd4 59 yh = my*cos(roll*PI/180) + mz*sin(roll*PI/180);
aolgu003 0:13ac527b4c4b 60 heading = atan2(yh, xh) * 180/PI;
aolgu003 0:13ac527b4c4b 61 }
aolgu003 0:13ac527b4c4b 62
aolgu003 0:13ac527b4c4b 63 void sensor_msg() {
aolgu003 0:13ac527b4c4b 64 //yaw,pitch,roll,raw heading, corrected heading, depth
aolgu003 0:13ac527b4c4b 65 pc.printf("%f,%f,%f,%f,%f,%f\r\n", yaw, pitch, roll, compass.getHeadingXYDeg(), heading, depth);
aolgu003 0:13ac527b4c4b 66 }
aolgu003 0:13ac527b4c4b 67
aolgu003 0:13ac527b4c4b 68 void print_data() {
aolgu003 0:13ac527b4c4b 69 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 70 pc.printf("Yaw, pitch, roll: %f, %f, %f\n", yaw, pitch, roll);
aolgu003 0:13ac527b4c4b 71 pc.printf("Depth: %f\n", depth);
aolgu003 0:13ac527b4c4b 72 pc.printf("Heading w.o. tilt correction: %f\n", compass.getHeadingXYDeg());
aolgu003 0:13ac527b4c4b 73 pc.printf("Heading with tilt correction: %f\n", heading);
aolgu003 0:13ac527b4c4b 74
aolgu003 0:13ac527b4c4b 75 }