Looking a little closer, it looks like he used the library from FreeIMU:
Can this be modified, or try a different library?
I just found it has it's own forum - Ill try ask over there.
/*
FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino
Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net>
*/
//#include <inttypes.h>
#include <stdint.h>
#include "main.h"
#include "FreeIMU.h"
#include "MPU6050.h"
//#include "vector_math.h"
/**
* Initialize the FreeIMU, sensors and performs gyro offsets calibration
*/
void FreeIMU_init(void) {
// disable internal pullups of the ATMEGA which Wire enable by default
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
// deactivate internal pull-ups for twi
// as per note from atmega8 manual pg167
cbi(PORTC, 4);
cbi(PORTC, 5);
#endif
// initialize quaternion
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
exInt = 0.0;
eyInt = 0.0;
ezInt = 0.0;
twoKp = twoKpDef;
twoKi = twoKiDef;
integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
gyro_off_x=0; gyro_off_y=0; gyro_off_z=0;
acc_off_x=0; acc_off_y=0; acc_off_z=0;
// initialize scale factors to neutral values
acc_scale_x = 1;
acc_scale_y = 1;
acc_scale_z = 1;
mpu6050_init();
//zero gyro
delay_ms(200);
FreeIMU_zeroGyro();
}//void FreeIMU_init(void)
/**
* Populates raw_values with the raw_values from the sensors
*/
void FreeIMU_getRawValues(int16_t * raw_values) {
mpu6050_read_all_axis_data(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
}
/**
* Populates values with calibrated readings from the sensors
*/
void FreeIMU_getValues(float * values) {
int i;
int16_t accgyroval[6];
mpu6050_read_all_axis_data(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
// remove offsets from the gyroscope
accgyroval[3] = accgyroval[3] - gyro_off_x;
accgyroval[4] = accgyroval[4] - gyro_off_y;
accgyroval[5] = accgyroval[5] - gyro_off_z;
for(i = 0; i<6; i++) {
if(i < 3) {
values[i] = (float) accgyroval[i];
}
else {
values[i] = ((float) accgyroval[i]) / GYRO_SENSITIVITY; // NOTE: this depends on the sensitivity chosen
}
}
// remove offsets and scale accelerometer (calibration)
values[0] = (values[0] - acc_off_x) / acc_scale_x;
values[1] = (values[1] - acc_off_y) / acc_scale_y;
values[2] = (values[2] - acc_off_z) / acc_scale_z;
//printf("%05f, %05f, %05f, %05f, %05f, %05f\r\n", values[0], values[1], values[2], values[3], values[4], values[5]);
}
/**
* Computes gyro offsets
*/
void FreeIMU_zeroGyro(void) {
int i;
const int totSamples = 30;
int16_t raw[11];
float tmpOffsets[] = {0,0,0};
for (i = 0; i < totSamples; i++){
FreeIMU_getRawValues(raw);
tmpOffsets[0] += raw[3];
tmpOffsets[1] += raw[4];
tmpOffsets[2] += raw[5];
delay_ms(10);
}
gyro_off_x = tmpOffsets[0] / totSamples;
gyro_off_y = tmpOffsets[1] / totSamples;
gyro_off_z = tmpOffsets[2] / totSamples;
printf("Gyro Offset: %05d, %05d, %05d\r\n", gyro_off_x, gyro_off_y, gyro_off_z);
}
/**
* Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion
* compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
* direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
* axis only.
*
* @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
*/
void FreeIMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
float qa, qb, qc;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
float halfvx, halfvy, halfvz;