File content as of revision 4:4b6f0c4cd39f:

#include "sensor_fusion.h"
#include "quaternion.h"
#include "millis.h"
#include "mbed.h"


MPU6050 mpu(SDA,SCL);

Serial pc(USBTX,USBRX,115200);


void normalize(struct vector *raw, struct vector *normalized);

int main() {
    mpu.start();
    millis_begin();
    struct vector orientation = {0, 0, 1}; //starting point
    struct vector gyro_previous = {0, 0, 0};
    struct vector gyro = {0, 0, 0};
    struct vector gyro_avg;
    struct vector xUnit = {1, 0, 0};
    struct vector yUnit = {0, 1, 0};
    struct vector zUnit = {0, 0, 1};
    struct quaternion xRot;
    struct quaternion yRot;
    struct quaternion zRot;
    struct quaternion finalRot;
    float time_current = 0;
    float time_previous = 0;
    float delta_time = 0;
    float radFactor = (3.14159265358979323846/180)/15.3;
    struct vector gAngularV = {0, 0, 0};
    
    
    while(1) {
        if (!mpu.data_ready()) {
            continue;
        }
        
        //raw data
        struct vector accel;


        
        //bias
        struct vector gyro_bias = {0, 0, 0};
        struct vector accel_bias = {500, 350, 2000};
        
        //gets raw data
        mpu.read_raw(&gyro.x, &gyro.y, &gyro.z, &accel.x, &accel.y, &accel.z);
        //pc.printf("Accel: (%f, %f, %f)\n Gyro: (%f, %f, %f)\r\n", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
        //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
        //pc.printf("%f %f %f\r\n",  gyro.x, gyro.y, gyro.z);
        
        //calculate angle
        time_current = (float(millis()) / 1000);
        delta_time = time_current - time_previous;
        vector_add(&gyro, &gyro_previous, &gyro_avg);
        vector_multiply(&gyro_avg, 0.5, &gAngularV);
        vector_multiply(&gAngularV, delta_time, &gAngularV);
        gyro_previous = gyro;
        time_previous = time_current;
        
        //rotate orientation vector
        gAngularV.x *= radFactor;
        gAngularV.y *= radFactor;
        gAngularV.z *= radFactor;
        quaternion_create(&xUnit, gAngularV.x, &xRot);
        quaternion_create(&yUnit, gAngularV.y, &yRot);
        quaternion_create(&zUnit, gAngularV.z, &zRot);
        quaternion_multiply(&xRot, &yRot, &finalRot);
        quaternion_multiply(&finalRot, &zRot, &finalRot);
        quaternion_rotate(&orientation, &finalRot, &orientation);
        vector_normalize(&orientation, &orientation);
        
        //adds bias
        vector_add(&accel, &accel_bias, &accel);
        vector_add(&gyro, &gyro_bias, &gyro);
        //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
        //pc.printf("%f %f %f\r\n",  gyro.x, gyro.y, gyro.z);
        wait(0.1);

        //normalize raw data to get vector
        struct vector accel_norm = {0, 0, 0};
        vector_normalize(&accel, &accel_norm);
        pc.printf("%f %f %f\r\n",  orientation.x, orientation.y, orientation.z);
        
        //wait(1);
    }
}