wrapper library for the Seagoat sensor suite
sensor.h
- Committer:
- aolgu003
- Date:
- 2016-07-23
- Revision:
- 0:13ac527b4c4b
- Child:
- 1:6126bf6cdfae
File content as of revision 0:13ac527b4c4b:
#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); }