sensor.h

00001 #include "MPU6050.h"
00002 #include "MS5803.h"
00003 #include "MS5837.h"
00004 #include "IMU.h"
00005 #include "HMC5883L.h"
00006 
00007 MPU6050 mpu6050;
00008 HMC5883L compass(I2C_SDA, I2C_SCL);
00009 float depth, heading;
00010 int16_t mag[3] = {0};
00011 
00012 
00013 void sensor_init() {
00014     IMUinit(mpu6050);
00015     wait_ms(100);
00016     
00017     pc.printf("================================\n");
00018     pc.printf("Initialized begining readings\n");
00019     pc.printf("================================\n");
00020 }
00021 
00022 void sensor_update() {
00023     //IMU Update gets pitch and roll
00024     IMUUpdate(mpu6050);
00025         
00026     //gets heading
00027     compass.getXYZ(mag);
00028     
00029     float xh, yh;
00030     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);
00031     yh = mag[1]*cos(roll*PI/180) + mag[2]*sin(roll*PI/180);
00032     heading = atan2(yh, xh) * 180/PI;
00033 }
00034 
00035 void sensor_msg() {
00036     //yaw,pitch,roll,raw heading, corrected heading, depth
00037     pc.printf("%f,%f,%f,%f,%f,%f\r\n", yaw, pitch, roll, compass.getHeadingXYDeg(), heading, depth);
00038 }
00039 
00040 void print_data() {
00041     pc.printf("================================\n");
00042     pc.printf("Yaw, pitch, roll: %f, %f, %f\n", yaw, pitch, roll);
00043     pc.printf("Depth: %f\n", depth);
00044     pc.printf("Heading w.o. tilt correction: %f\n", compass.getHeadingXYDeg());
00045     pc.printf("Heading with tilt correction: %f\n", heading);
00046 
00047 }