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