Sonya Ragothaman / sensor

Sonya's revisions to sensor.h

Fork of sensor by Andrew Olguin

sensor.h@0:13ac527b4c4b, 2016-07-23 (annotated)

Committer:
aolgu003
Date:
Sat Jul 23 04:15:42 2016 +0000
Revision:
0:13ac527b4c4b
Child:
1:6126bf6cdfae
Updated and functioning sensor library. Added HMC5883L compass for heading calculation and MS5837 for depth calculation. Tilt and offset corrections for pressure sensor still need to be added for depth calculation along with low pass filtering forIMU

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"
aolgu003 0:13ac527b4c4b 3 #include "MS5837.h"
aolgu003 0:13ac527b4c4b 4 #include "IMU.h"
aolgu003 0:13ac527b4c4b 5 #include "HMC5883L.h"
aolgu003 0:13ac527b4c4b 6
aolgu003 0:13ac527b4c4b 7 MPU6050 mpu6050;
aolgu003 0:13ac527b4c4b 8 HMC5883L compass(PB_4, PA_8);
aolgu003 0:13ac527b4c4b 9 MS5837 pressure_sensor_1 = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS);
aolgu003 0:13ac527b4c4b 10 MS5837 pressure_sensor_2 = MS5837(PB_4, PA_8, ms5837_addr_no_CS);
aolgu003 0:13ac527b4c4b 11 float depth, depth1, depth2, heading;
aolgu003 0:13ac527b4c4b 12 float pressure1, pressure2;
aolgu003 0:13ac527b4c4b 13 int error_cnt = 0;
aolgu003 0:13ac527b4c4b 14 int16_t mag[3] = {0};
aolgu003 0:13ac527b4c4b 15
aolgu003 0:13ac527b4c4b 16
aolgu003 0:13ac527b4c4b 17 void sensor_init() {
aolgu003 0:13ac527b4c4b 18 wait_ms(100);
aolgu003 0:13ac527b4c4b 19 pressure_sensor_1.MS5837Init();
aolgu003 0:13ac527b4c4b 20 wait_ms(100);
aolgu003 0:13ac527b4c4b 21 pressure_sensor_2.MS5837Init();
aolgu003 0:13ac527b4c4b 22 wait_ms(100);
aolgu003 0:13ac527b4c4b 23 IMUinit(mpu6050);
aolgu003 0:13ac527b4c4b 24 wait_ms(100);
aolgu003 0:13ac527b4c4b 25
aolgu003 0:13ac527b4c4b 26 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 27 pc.printf("Initialized begining readings\n");
aolgu003 0:13ac527b4c4b 28 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 29 }
aolgu003 0:13ac527b4c4b 30
aolgu003 0:13ac527b4c4b 31 void sensor_update() {
aolgu003 0:13ac527b4c4b 32 //IMU Update gets pitch and roll
aolgu003 0:13ac527b4c4b 33 IMUUpdate(mpu6050);
aolgu003 0:13ac527b4c4b 34
aolgu003 0:13ac527b4c4b 35 //Finds depth
aolgu003 0:13ac527b4c4b 36 //pressure_sensor_1.Barometer_MS5837();
aolgu003 0:13ac527b4c4b 37 pressure_sensor_2.Barometer_MS5837();
aolgu003 0:13ac527b4c4b 38 //pressure1 = pressure_sensor_1.MS5837_Pressure();
aolgu003 0:13ac527b4c4b 39 pressure2 = pressure_sensor_2.MS5837_Pressure();
aolgu003 0:13ac527b4c4b 40 depth = pressure_sensor_2.depth();
aolgu003 0:13ac527b4c4b 41
aolgu003 0:13ac527b4c4b 42 /*//Do some error checking
aolgu003 0:13ac527b4c4b 43 if ((pressure1 > 900 && pressure1 < 3000) && (pressure2 > 900 && pressure2 < 3000)) {
aolgu003 0:13ac527b4c4b 44 //depth1 = pressure_sensor_1.depth();
aolgu003 0:13ac527b4c4b 45 depth2 = pressure_sensor_2.depth();
aolgu003 0:13ac527b4c4b 46
aolgu003 0:13ac527b4c4b 47 depth = (depth1 + depth2) / 2;
aolgu003 0:13ac527b4c4b 48 error_cnt = 0;
aolgu003 0:13ac527b4c4b 49 } else if (pressure2 > 900 && pressure2 < 3000) {
aolgu003 0:13ac527b4c4b 50 depth = pressure_sensor_2.depth();
aolgu003 0:13ac527b4c4b 51
aolgu003 0:13ac527b4c4b 52 error_cnt = 0;
aolgu003 0:13ac527b4c4b 53 } else if (pressure1 > 900 && pressure1 < 3000) {
aolgu003 0:13ac527b4c4b 54 //depth = pressure_sensor_1.depth();
aolgu003 0:13ac527b4c4b 55 error_cnt = 0;
aolgu003 0:13ac527b4c4b 56 } else {
aolgu003 0:13ac527b4c4b 57 error_cnt++;
aolgu003 0:13ac527b4c4b 58 if (error_cnt > 50) {
aolgu003 0:13ac527b4c4b 59 //do something
aolgu003 0:13ac527b4c4b 60 pc.printf("Pressure sensor error");
aolgu003 0:13ac527b4c4b 61 }
aolgu003 0:13ac527b4c4b 62 }*/
aolgu003 0:13ac527b4c4b 63
aolgu003 0:13ac527b4c4b 64 //gets heading
aolgu003 0:13ac527b4c4b 65 compass.getXYZ(mag);
aolgu003 0:13ac527b4c4b 66
aolgu003 0:13ac527b4c4b 67 float xh, yh;
aolgu003 0:13ac527b4c4b 68 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);
aolgu003 0:13ac527b4c4b 69 yh = mag[1]*cos(roll*PI/180) + mag[2]*sin(roll*PI/180);
aolgu003 0:13ac527b4c4b 70 heading = atan2(yh, xh) * 180/PI;
aolgu003 0:13ac527b4c4b 71 }
aolgu003 0:13ac527b4c4b 72
aolgu003 0:13ac527b4c4b 73 void sensor_msg() {
aolgu003 0:13ac527b4c4b 74 //yaw,pitch,roll,raw heading, corrected heading, depth
aolgu003 0:13ac527b4c4b 75 pc.printf("%f,%f,%f,%f,%f,%f\r\n", yaw, pitch, roll, compass.getHeadingXYDeg(), heading, depth);
aolgu003 0:13ac527b4c4b 76 }
aolgu003 0:13ac527b4c4b 77
aolgu003 0:13ac527b4c4b 78 void print_data() {
aolgu003 0:13ac527b4c4b 79 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 80 pc.printf("Yaw, pitch, roll: %f, %f, %f\n", yaw, pitch, roll);
aolgu003 0:13ac527b4c4b 81 pc.printf("Depth 1: %f\n", depth1);
aolgu003 0:13ac527b4c4b 82 pc.printf("Depth 2: %f\n", depth2);
aolgu003 0:13ac527b4c4b 83 pc.printf("Depth: %f\n", depth);
aolgu003 0:13ac527b4c4b 84 pc.printf("Heading w.o. tilt correction: %f\n", compass.getHeadingXYDeg());
aolgu003 0:13ac527b4c4b 85 pc.printf("Heading with tilt correction: %f\n", heading);
aolgu003 0:13ac527b4c4b 86
aolgu003 0:13ac527b4c4b 87 }