File content as of revision 2:be5fd562cbd4:

#include "MPU6050.h"
#include "MS5803.h"
//#include "MS5837.h"
#include "IMU.h"
#include "HMC5883L.h"

#include <string>
#include <vector>

Serial device(PA_9,PA_10); // pressure sensor
MPU6050 mpu6050;
HMC5883L compass(I2C_SDA, I2C_SCL);
float depth = 0.0, heading;
int16_t mag[3] = {0};


void callback(){                // pressure sensor interrupt
    char a = 0;
    char i = 0;
    char buffer[10] = {' '};
    if (device.readable()) {
        while(a != 'd') {
            a = device.getc();
            if ((a >= '0' && a <='9') || a == '.'){
                    buffer[i] = a;
                    i++;
            }
        }
        depth = atof(buffer);
        //pc.printf("Depth: '%f'\n", depth); 
    }
    
} 

void sensor_init() {
    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);
        
    //gets heading
    compass.getXYZ(mag);
    
    float mx,my,mz;
    mx = mag[1];
    my = -1*mag[0];
    mz = mag[2];
    
    
    float xh, yh;
    xh = mx*cos(pitch*PI/180) + my*sin(roll*PI/180)*sin(pitch*PI/180) - mz*cos(roll*PI/180)*sin(pitch*PI/180);
    yh = my*cos(roll*PI/180) + mz*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: %f\n", depth);
    pc.printf("Heading w.o. tilt correction: %f\n", compass.getHeadingXYDeg());
    pc.printf("Heading with tilt correction: %f\n", heading);

}