wrapper library for the Seagoat sensor suite
Diff: sensor.h
- Revision:
- 0:13ac527b4c4b
- Child:
- 1:6126bf6cdfae
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sensor.h Sat Jul 23 04:15:42 2016 +0000 @@ -0,0 +1,87 @@ +#include "MPU6050.h" +#include "MS5803.h" +#include "MS5837.h" +#include "IMU.h" +#include "HMC5883L.h" + +MPU6050 mpu6050; +HMC5883L compass(PB_4, PA_8); +MS5837 pressure_sensor_1 = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS); +MS5837 pressure_sensor_2 = MS5837(PB_4, PA_8, ms5837_addr_no_CS); +float depth, depth1, depth2, heading; +float pressure1, pressure2; +int error_cnt = 0; +int16_t mag[3] = {0}; + + +void sensor_init() { + wait_ms(100); + pressure_sensor_1.MS5837Init(); + wait_ms(100); + pressure_sensor_2.MS5837Init(); + wait_ms(100); + IMUinit(mpu6050); + wait_ms(100); + + pc.printf("================================\n"); + pc.printf("Initialized begining readings\n"); + pc.printf("================================\n"); +} + +void sensor_update() { + //IMU Update gets pitch and roll + IMUUpdate(mpu6050); + + //Finds depth + //pressure_sensor_1.Barometer_MS5837(); + pressure_sensor_2.Barometer_MS5837(); + //pressure1 = pressure_sensor_1.MS5837_Pressure(); + pressure2 = pressure_sensor_2.MS5837_Pressure(); + depth = pressure_sensor_2.depth(); + + /*//Do some error checking + if ((pressure1 > 900 && pressure1 < 3000) && (pressure2 > 900 && pressure2 < 3000)) { + //depth1 = pressure_sensor_1.depth(); + depth2 = pressure_sensor_2.depth(); + + depth = (depth1 + depth2) / 2; + error_cnt = 0; + } else if (pressure2 > 900 && pressure2 < 3000) { + depth = pressure_sensor_2.depth(); + + error_cnt = 0; + } else if (pressure1 > 900 && pressure1 < 3000) { + //depth = pressure_sensor_1.depth(); + error_cnt = 0; + } else { + error_cnt++; + if (error_cnt > 50) { + //do something + pc.printf("Pressure sensor error"); + } + }*/ + + //gets heading + compass.getXYZ(mag); + + 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); + heading = atan2(yh, xh) * 180/PI; +} + +void sensor_msg() { + //yaw,pitch,roll,raw heading, corrected heading, depth + pc.printf("%f,%f,%f,%f,%f,%f\r\n", yaw, pitch, roll, compass.getHeadingXYDeg(), heading, depth); +} + +void print_data() { + pc.printf("================================\n"); + pc.printf("Yaw, pitch, roll: %f, %f, %f\n", yaw, pitch, roll); + pc.printf("Depth 1: %f\n", depth1); + pc.printf("Depth 2: %f\n", depth2); + pc.printf("Depth: %f\n", depth); + pc.printf("Heading w.o. tilt correction: %f\n", compass.getHeadingXYDeg()); + pc.printf("Heading with tilt correction: %f\n", heading); + +}