Sonya's revisions to sensor.h
Fork of sensor by
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?
User | Revision | Line number | New 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 | } |