Drift Problem 9DOF Headtracker

Hello Guys,

I have a problem with my current project reproducing the IMU Headtracker vom Gregory Pauls Hackaday Site (Instructions | Elite Dangerous Headtracker | Hackaday.io) using a Teensy++2.0.

Problem is, the MiniIMU board from Pololu changed in the meantime to v5, (Pololu - MinIMU-9 v5 Gyro, Accelerometer, and Compass (LSM6DS33 and LIS3MDL Carrier)) using a LSM6DS33 (3 axis gyro and 3 axis accelerometer) and a LIS3MDL (3 axis magnetometer). So we have now different sensors and the accelerometer is now integretated in the gyro and not in the magnetometer anymore.

I changed the code, integrating the new libraries (with a change from “int” to “float” for one parameter to match the smoothing function) and reading the Acc-data from the gyro (gyro.a.x instead of compass.a.x).

The code works in general pretty good, but it starts drifting soon (especially the yaw-anlge) - in my understanding the 3-axis-magnetometer should prevent the drift and reset the direction? Apperently this doesn´t work and i have no idea why. In certain positions the YAW and ROLL Values keep changing, even if the device is kept still.
It also seems, that there is no change to the drift if I delete the magnetometer-setup in the IMUInit-function. I calibrated the magnetmoter and put the values in the beginning headtracker-code.

Can someone help me please? Here is my altered code:


/*
  Code below comes from http://planetkris.com/2014/12/easier-better-arduino-imu-head-tracker/ with minors updates.

  Based on the Madgwick algorithm found at:
  See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

  This code inherits all relevant licenses and may be freely modified and redistributed.
  The MinIMU v1 has a roughly +/- 10degree accuracy
  The MinIMU v2 has a roughly +/- 1 degree accuracy
*/

//Uncommenting following line will enable debugging output
#define DEBUG

#include <LIS3MDL.h>                                              //Change                                     
#include <LSM6.h>                                                 //Change
#include <Wire.h>

#define ToRad(x) ((x) * 0.01745329252)  // *pi/180
#define ToDeg(x) ((x) * 57.2957795131)  // *180/pi
#define PI_FLOAT     3.14159265f
#define PIBY2_FLOAT  1.5707963f
#define GYRO_SCALE 0.07f
#define betaDef    0.08f

/*
 * To find the calibration values, load the Calibrate sketch from LSM303 library in menu : examples > LSM303 > Calibrate
 * https://github.com/pololu/lsm303-arduino/blob/master/LSM303/examples/Calibrate/Calibrate.ino
 * Then put the calibration values below
 */
#define compassXMin -3472.0f
#define compassYMin -4107.0f
#define compassZMin -4935.0f
#define compassXMax 2880.0f
#define compassYMax 2889.0f
#define compassZMax 1572.0f
#define inverseXRange (float)(2.0 / (compassXMax - compassXMin))
#define inverseYRange (float)(2.0 / (compassYMax - compassYMin))
#define inverseZRange (float)(2.0 / (compassZMax - compassZMin))

#define headingtimeout 20000  //20 sec in ms
// Teensy 2.0 has the LED on pin 11
// Teensy++ 2.0 has the LED on pin 6
// Teensy 3.0 has the LED on pin 13
const int ledPin = 6;

LSM6 gyro;                                                                //Change
LIS3MDL compass;                                                          //Change

long timer, printTimer;
float G_Dt;
int loopCount;
int blinkCount;

float q0;
float q1;
float q2;
float q3;
float beta;

float magnitude;

float pitch, roll, yaw;

float gyroSumX, gyroSumY, gyroSumZ;
float offSetX, offSetY, offSetZ;

float floatMagX, floatMagY, floatMagZ;
float smoothAccX, smoothAccY, smoothAccZ;
float accToFilterX, accToFilterY, accToFilterZ;

int i;
int inithead;  //Store initial heading
boolean fixed;  //Lock in that heading
int newhead;  //360 degree transformation

void setup() {
#ifdef DEBUG
  Serial.begin(115200);
  Serial.println("Keeping the device still and level during startup will yield the best results");
#endif
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH);   // set the LED on
  delay(100);
  Wire.begin();
  TWBR = ((F_CPU / 400000) - 16) / 2;//set the I2C speed to 400KHz
  IMUinit();
#ifdef DEBUG
  Serial.println("IMUinit done");
#endif
  printTimer = millis();
  timer = micros();
  blinkCount = 0;
  inithead = yaw; // Grab an initial heading - expect this to change
  fixed = false;
}

void loop() {
  if (micros() - timer >= 5000) {
    
//this runs in 4ms on the MEGA 2560

    G_Dt = (micros() - timer) / 1000000.0;
    timer = micros();
    
    compass.read();
        floatMagX = ((float)compass.m.x - compassXMin) * inverseXRange - 1.0;
        floatMagY = ((float)compass.m.y - compassYMin) * inverseYRange - 1.0;
        floatMagZ = ((float)compass.m.z - compassZMin) * inverseZRange - 1.0;

    gyro.read();
        Smoothing(&gyro.a.x, &smoothAccX);                                                                       //Change
        Smoothing(&gyro.a.y, &smoothAccY);                                                                       //Change
        Smoothing(&gyro.a.z, &smoothAccZ);                                                                       //Change
        accToFilterX = smoothAccX;                                                                               //Change
        accToFilterY = smoothAccY;                                                                               //Change           
        accToFilterZ = smoothAccZ;                                                                               //Change
        AHRSupdate(&G_Dt);
        
//update the joystick heading coordinates
// modify degress for heading

    newhead = yaw - inithead;
    if (newhead > 180) {
      newhead = -360 + newhead;
    }
    else if (newhead < -180) {
      newhead = 360 + newhead;
    }
    if (newhead < 0) {
      newhead = fscale(-90, 0, 0, 512, newhead, 0);
    }
    else {
      newhead = fscale(0, 90, 512, 1023, newhead, 0);
    }
    if (newhead < 0) {
      newhead = 0;
    }
    if (newhead > 1023) {
      newhead = 1023;
    }
    Joystick.X(newhead);
  }

  
  if (millis() - printTimer > 10) {
    printTimer = millis();
    GetEuler();
    
    if (roll < 0) {
      roll = fscale(-90, 0, 0, 512, roll, 0);
    }
    else {
      roll = fscale(0, 90, 512, 1023, roll, 0);
    }
    if (roll < 0) {
      roll = 0;
    }
    if (roll > 1023) {
      roll = 1023;
    }
    Joystick.Z(roll);
    
    if (pitch < 0) {
      pitch = fscale(-90, 0, 0, 512, pitch, 0);
    }
    else {
      pitch = fscale(0, 90, 512, 1023, pitch, 0);
    }
    if (pitch < 0) {
      pitch = 0;
    }
    if (pitch > 1023) {
      pitch = 1023;
    }
    Joystick.Y(pitch);

#ifdef DEBUG
    Serial.print("newhead: ");
    Serial.print(newhead);
    Serial.print(", pitch: ");
    Serial.print(pitch);
    Serial.print(", roll: ");
    Serial.println(roll);
#endif

    if (fixed == false) {
      blinkCount++;
      if (blinkCount > 15) {
        digitalWrite(ledPin, HIGH);
      }
      if (blinkCount > 30) {
        digitalWrite(ledPin, LOW);
        blinkCount = 0;
      }
      if (printTimer > headingtimeout) {
        inithead = yaw;
        fixed = true;
        digitalWrite(ledPin, HIGH);   // set the LED on
      }
    }
  }
}

void IMUinit() {
  //init devices
  compass.init();
  gyro.init();
  
  gyro.writeReg(0x19, 0x38);                                                              //Change
  gyro.writeReg(0x11, 0x6C);                                                              //Change
  gyro.writeReg(0x0D, 0x02);                                                              //Change
                                                          
  gyro.writeReg(0x18, 0x38);                                                              //Change
  gyro.writeReg(0x10, 0x6C);                                                              //Change
  gyro.writeReg(0x0D, 0x01);                                                              //Change                                                            

 /compass.writeReg(0x20, 0xFC);                                                           //Change   
  compass.writeReg(0x21, 0x00);                                                           //Change  
  compass.writeReg(0x22, 0x00);                                                           //Change
  compass.writeReg(0x23, 0x0C);                                                           //Change

  beta = betaDef;
  //calculate initial quaternion
  //take an average of the gyro readings to remove the bias

  for (i = 0; i < 500; i++) {
    gyro.read();
    Smoothing(&gyro.a.x, &smoothAccX);                         //Change
    Smoothing(&gyro.a.y, &smoothAccY);                         //Change
    Smoothing(&gyro.a.z, &smoothAccZ);                         //Change
    compass.read();
    delay(3);
  }
  gyroSumX = 0;
  gyroSumY = 0;
  gyroSumZ = 0;
  for (i = 0; i < 500; i++) {
    gyro.read();                                              ///Change
    Smoothing(&gyro.a.x, &smoothAccX);                         //Change
    Smoothing(&gyro.a.y, &smoothAccY);                         //Change
    Smoothing(&gyro.a.z, &smoothAccZ);                         //Change
    compass.read();

    gyroSumX += (gyro.g.x);
    gyroSumY += (gyro.g.y);
    gyroSumZ += (gyro.g.z);
    delay(3);
  }
  offSetX = gyroSumX / 500.0;
  offSetY = gyroSumY / 500.0;
  offSetZ = gyroSumZ / 500.0;
  gyro.read();                                          //Change

  //calculate the initial quaternion
  //these are rough values. This calibration works a lot better if the device is kept as flat as possible
  //find the initial pitch and roll
  pitch = ToDeg(fastAtan2(gyro.a.x, sqrt(gyro.a.y * gyro.a.y + gyro.a.z * gyro.a.z)));                             //Change
  roll = ToDeg(fastAtan2(-1 * gyro.a.y, sqrt(gyro.a.x * gyro.a.x + gyro.a.z * gyro.a.z)));                         //Change


  if (gyro.a.z > 0) {                                                                                                    //Change
    if (gyro.a.x > 0) {                                                                                                      //Change
      pitch = 180.0 - pitch;
    }
    else {
      pitch = -180.0 - pitch;
    }
    if (gyro.a.y > 0) {                                                                                                    //Change
      roll = -180.0 - roll;
    }
    else {
      roll = 180.0 - roll;
    }
  }

  floatMagX = (compass.m.x - compassXMin) * inverseXRange - 1.0;
  floatMagY = (compass.m.y - compassYMin) * inverseYRange - 1.0;
  floatMagZ = (compass.m.z - compassZMin) * inverseZRange - 1.0;
  //tilt compensate the compass
  float xMag = (floatMagX * cos(ToRad(pitch))) + (floatMagZ * sin(ToRad(pitch)));
  float yMag = -1 * ((floatMagX * sin(ToRad(roll))  * sin(ToRad(pitch))) + (floatMagY * cos(ToRad(roll))) - (floatMagZ * sin(ToRad(roll)) * cos(ToRad(pitch))));

  yaw = ToDeg(fastAtan2(yMag, xMag));

  if (yaw < 0) {
    yaw += 360;
  }
  //calculate the rotation matrix
  float cosPitch = cos(ToRad(pitch));
  float sinPitch = sin(ToRad(pitch));

  float cosRoll = cos(ToRad(roll));
  float sinRoll = sin(ToRad(roll));

  float cosYaw = cos(ToRad(yaw));
  float sinYaw = sin(ToRad(yaw));

  //need the transpose of the rotation matrix
  float r11 = cosPitch * cosYaw;
  float r21 = cosPitch * sinYaw;
  float r31 = -1.0 * sinPitch;

  float r12 = -1.0 * (cosRoll * sinYaw) + (sinRoll * sinPitch * cosYaw);
  float r22 = (cosRoll * cosYaw) + (sinRoll * sinPitch * sinYaw);
  float r32 = sinRoll * cosPitch;

  float r13 = (sinRoll * sinYaw) + (cosRoll * sinPitch * cosYaw);
  float r23 = -1.0 * (sinRoll * cosYaw) + (cosRoll * sinPitch * sinYaw);
  float r33 = cosRoll * cosPitch;

  //convert to quaternion
  q0 = 0.5 * sqrt(1 + r11 + r22 + r33);
  q1 = (r32 - r23) / (4 * q0);
  q2 = (r13 - r31) / (4 * q0);
  q3 = (r21 - r12) / (4 * q0);
}

void IMUupdate(float *dt) {
  static float gx;
  static float gy;
  static float gz;
  static float ax;
  static float ay;
  static float az;

  static float recipNorm;
  static float s0, s1, s2, s3;
  static float qDot1, qDot2, qDot3, qDot4;
  static float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 , _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

  gx = ToRad((gyro.g.x - offSetX) * GYRO_SCALE);
  gy = ToRad((gyro.g.y - offSetY) * GYRO_SCALE);
  gz = ToRad((gyro.g.z - offSetZ) * GYRO_SCALE);

  ax = -1.0 * gyro.a.x;                                                                                                           //Change
  ay = -1.0 * gyro.a.y;                                                                                                    //Change
  az = -1.0 * gyro.a.z;                                                                                                    //Change
  // Rate of change of quaternion from gyroscope
  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

  magnitude = sqrt(ax * ax + ay * ay + az * az);
  if ((magnitude > 384) || (magnitude < 128)) {
    ax = 0;
    ay = 0;
    az = 0;
  }

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Auxiliary variables to avoid repeated arithmetic
    _2q0 = 2.0f * q0;
    _2q1 = 2.0f * q1;
    _2q2 = 2.0f * q2;
    _2q3 = 2.0f * q3;
    _4q0 = 4.0f * q0;
    _4q1 = 4.0f * q1;
    _4q2 = 4.0f * q2;
    _8q1 = 8.0f * q1;
    _8q2 = 8.0f * q2;
    q0q0 = q0 * q0;
    q1q1 = q1 * q1;
    q2q2 = q2 * q2;
    q3q3 = q3 * q3;

    // Gradient decent algorithm corrective step
    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
    s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
    s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
    s0 *= recipNorm;
    s1 *= recipNorm;
    s2 *= recipNorm;
    s3 *= recipNorm;

    // Apply feedback step
    qDot1 -= beta * s0;
    qDot2 -= beta * s1;
    qDot3 -= beta * s2;
    qDot4 -= beta * s3;
  }

  // Integrate rate of change of quaternion to yield quaternion
  q0 += qDot1 * *dt;
  q1 += qDot2 * *dt;
  q2 += qDot3 * *dt;
  q3 += qDot4 * *dt;

  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

void AHRSupdate(float *dt) {
  static float gx;
  static float gy;
  static float gz;
  static float ax;
  static float ay;
  static float az;
  static float mx;
  static float my;
  static float mz;

  static float recipNorm;
  static float s0, s1, s2, s3;
  static float qDot1, qDot2, qDot3, qDot4;
  static float hx, hy;
  static float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

  gx = ToRad((gyro.g.x - offSetX) * GYRO_SCALE);
  gy = ToRad((gyro.g.y - offSetY) * GYRO_SCALE);
  gz = ToRad((gyro.g.z - offSetZ) * GYRO_SCALE);

  ax = -1.0 * gyro.a.x;
  ay = -1.0 * gyro.a.y;
  az = -1.0 * gyro.a.z;

  mx = floatMagX;
  my = floatMagY;
  mz = floatMagZ;
  // Rate of change of quaternion from gyroscope
  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

  magnitude = sqrt(ax * ax + ay * ay + az * az);

  if ((magnitude > 384) || (magnitude < 128)) {
    ax = 0;
    ay = 0;
    az = 0;
  }

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    // Normalise magnetometer measurement
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;
    // Auxiliary variables to avoid repeated arithmetic
    _2q0mx = 2.0f * q0 * mx;
    _2q0my = 2.0f * q0 * my;
    _2q0mz = 2.0f * q0 * mz;
    _2q1mx = 2.0f * q1 * mx;
    _2q0 = 2.0f * q0;
    _2q1 = 2.0f * q1;
    _2q2 = 2.0f * q2;
    _2q3 = 2.0f * q3;
    _2q0q2 = 2.0f * q0 * q2;
    _2q2q3 = 2.0f * q2 * q3;
    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;

    // Reference direction of Earth's magnetic field
    hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
    hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
    _2bx = sqrt(hx * hx + hy * hy);
    _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
    _4bx = 2.0f * _2bx;
    _4bz = 2.0f * _2bz;

    // Gradient decent algorithm corrective step
    s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
    s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
    s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
    s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
    s0 *= recipNorm;
    s1 *= recipNorm;
    s2 *= recipNorm;
    s3 *= recipNorm;

    // Apply feedback step
    qDot1 -= beta * s0;
    qDot2 -= beta * s1;
    qDot3 -= beta * s2;
    qDot4 -= beta * s3;
  }

  // Integrate rate of change of quaternion to yield quaternion
  q0 += qDot1 * *dt;
  q1 += qDot2 * *dt;
  q2 += qDot3 * *dt;
  q3 += qDot4 * *dt;

  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

void GetEuler(void) {
  roll = ToDeg(fastAtan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)));
  pitch = ToDeg(asin(2 * (q0 * q2 - q3 * q1)));
  yaw = ToDeg(fastAtan2(2 * (q0 * q3 + q1 * q2) , 1 - 2 * (q2 * q2 + q3 * q3)));
  if (yaw < 0) {
    yaw += 360;
  }

}
float fastAtan2( float y, float x)
{
  static float atan;
  static float z;
  if ( x == 0.0f )
  {
    if ( y > 0.0f ) return PIBY2_FLOAT;
    if ( y == 0.0f ) return 0.0f;
    return -PIBY2_FLOAT;
  }
  z = y / x;
  if ( fabs( z ) < 1.0f )
  {
    atan = z / (1.0f + 0.28f * z * z);
    if ( x < 0.0f )
    {
      if ( y < 0.0f ) return atan - PI_FLOAT;
      return atan + PI_FLOAT;
    }
  }
  else
  {
    atan = PIBY2_FLOAT - z / (z * z + 0.28f);
    if ( y < 0.0f ) return atan - PI_FLOAT;
  }
  return atan;
}

float invSqrt(float number) {
  volatile long i;
  volatile float x, y;
  volatile const float f = 1.5F;

  x = number * 0.5F;
  y = number;
  i = * ( long * ) &y;
  i = 0x5f375a86 - ( i >> 1 );
  y = * ( float * ) &i;
  y = y * ( f - ( x * y * y ) );
  return y;
}

void Smoothing(float *raw, float *smooth) {
  *smooth = (*raw * (0.15)) + (*smooth * 0.85);
}

float fscale( float originalMin, float originalMax, float newBegin, float newEnd, float inputValue, float curve) {
  float OriginalRange = 0;
  float NewRange = 0;
  float zeroRefCurVal = 0;
  float normalizedCurVal = 0;
  float rangedValue = 0;
  boolean invFlag = 0;


  // condition curve parameter
  // limit range

  if (curve > 10) curve = 10;
  if (curve < -10) curve = -10;

  curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output
  curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function

  // Check for out of range inputValues
  if (inputValue < originalMin) {
    inputValue = originalMin;
  }
  if (inputValue > originalMax) {
    inputValue = originalMax;
  }

  // Zero Refference the values
  OriginalRange = originalMax - originalMin;

  if (newEnd > newBegin) {
    NewRange = newEnd - newBegin;
  }
  else
  {
    NewRange = newBegin - newEnd;
    invFlag = 1;
  }

  zeroRefCurVal = inputValue - originalMin;
  normalizedCurVal  =  zeroRefCurVal / OriginalRange;   // normalize to 0 - 1 float

  // Check for originalMin > originalMax  - the math for all other cases i.e. negative numbers seems to work out fine
  if (originalMin > originalMax ) {
    return 0;
  }

  if (invFlag == 0) {
    rangedValue =  (pow(normalizedCurVal, curve) * NewRange) + newBegin;

  }
  else // invert the ranges
  {
    rangedValue =  newBegin - (pow(normalizedCurVal, curve) * NewRange);
  }

  return rangedValue;
}

Thank you so much,
durzo92

1 Like

Did you calibrate the magnetometer? If not, the data are usually useless.

Yes, I loaded the calibration-code on the controller, put the serial monitor on, turned the device in every direction and put the MINxyz, MAXxyz-Values in the beginning of the headtracker code.

I finally took a close look at the code, and this is all wrong. I can’t imagine how it ever could have worked. You are really just wasting your time with this junk.

    compass.read();
        floatMagX = ((float)compass.m.x - compassXMin) * inverseXRange - 1.0;
        floatMagY = ((float)compass.m.y - compassYMin) * inverseYRange - 1.0;
        floatMagZ = ((float)compass.m.z - compassZMin) * inverseZRange - 1.0;

    gyro.read();
        Smoothing(&gyro.a.x, &smoothAccX);                                                                       //Change
        Smoothing(&gyro.a.y, &smoothAccY);                                                                       //Change
        Smoothing(&gyro.a.z, &smoothAccZ);                                                                       //Change
        accToFilterX = smoothAccX;                                                                               //Change
        accToFilterY = smoothAccY;                                                                               //Change           
        accToFilterZ = smoothAccZ;                                                                               //Change
        AHRSupdate(&G_Dt);

I suggest to find a sensible headtracker example. Or, write your own. The first step is to get an AHRS working with your sensor (there are several possibilities on line). Pololu offers one, and here is another. When you end up with something that returns sensible, understandable angles, move on from that point.

1 Like

Allright, thank you for your input! I´ll look into it and try to put it up correctly - thanks! :slight_smile:

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.