Problem in reading for AHRS of Adafruit 10 DOF IMU

Breakout boards, sensors, other Adafruit kits, etc.

Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.
Locked
User avatar
Hambalala
 
Posts: 9
Joined: Sun Feb 08, 2015 2:56 pm

Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by Hambalala »

Hello there,

I started using Adafruit 10 DOF IMU breakout for a while and is stuck with a problem that needs urgent solution. The problem I am facing is while using the AHRS code for 10DOF provided by Adafruit.

During the reading if I rotate the sensor along z-axis, I get close to zero values in pitch and roll and an angle count in yaw.
Also while tilting the sensor along the y-axis, I get close to zero values in pitch and yaw and an angle count in roll.
However, while taking the reading along the x-axis, I get a lot of drifting values in yaw and roll which does not make sense to me any more. I have attached pictures. Should the result not be close to zero values for yaw and roll and an angle count for pitch?

I will really appreciate help in this matter.
Attachments
This reading is taken by putting the sensor tilting along -(ve) x axis
This reading is taken by putting the sensor tilting along -(ve) x axis
Tilt data 2.PNG (43.97 KiB) Viewed 2429 times
This reading is taken by putting the sensor tilting along +(ve) x axis
This reading is taken by putting the sensor tilting along +(ve) x axis
Tilt data.PNG (46.35 KiB) Viewed 2429 times

User avatar
ktownsend
 
Posts: 1447
Joined: Thu Nov 05, 2009 2:18 am

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by ktownsend »

The drift you see is most likely caused by the fact that the magnetometer used in the calculations is not calibrated for hard iron or soft iron errors. We have plans to introduce a calibration utility as well as integrate the gyroscope data (using a common algorithm like Magdwick, etc.), but at present we don't have an example offering full integration with this chipset.

I'll try to move this up the ToDo list since it's been sitting there too long.

User avatar
Hambalala
 
Posts: 9
Joined: Sun Feb 08, 2015 2:56 pm

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by Hambalala »

I appreciate your reply. Can you please suggest me a step-wise procedure of approaching this problem with other kind of algorithm? and what kind of calibration utility I could use in this case?

User avatar
ktownsend
 
Posts: 1447
Joined: Thu Nov 05, 2009 2:18 am

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by ktownsend »

There are a lot of DOF solutions out there. I'd suggest doing a quick search for "Madgwick" plus Arduino since that's probably the most appropriate algorithm for 9DOF sensor fusion. It's light weight (compared to other solutions) and several code examples are floating around out there.

Here is an explanation of how calibration works for magnetometers, and this blog is also an excellent place to start looking at some of the implementation details around AHRS: http://www.camelsoftware.com/firetail/b ... ft-errors/

K.

User avatar
Hambalala
 
Posts: 9
Joined: Sun Feb 08, 2015 2:56 pm

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by Hambalala »

Thanks for your support. Just a question: Do you have idea on when you guys are going to implement it?

User avatar
waltmck
 
Posts: 25
Joined: Sat Jul 21, 2012 11:54 am

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by waltmck »

ktownsend said
We have plans to introduce a calibration utility as well as integrate the gyroscope data (using a common algorithm like Magdwick, etc.), but at present we don't have an example offering full integration with this chipset.

I'll try to move this up the ToDo list since it's been sitting there too long.
I'll second getting this is place. Mainly the ability to feed calibration values back into the libraries. Without the ability to do this I've found the pitch/roll/orientation functions to be not very useful. My sensor gives a steady roll of 6 degrees when sitting on my fairly level window sill.

Doing the calibrations and using the Mahoney algorithm I'm getting accurate and steady readings for roll and pitch from 0 to +/- 60 degrees. Less luck so far with the Madgwick.

User avatar
Hambalala
 
Posts: 9
Joined: Sun Feb 08, 2015 2:56 pm

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by Hambalala »

Hey waltmck, how are you using the Mahony algorithm?

User avatar
waltmck
 
Posts: 25
Joined: Sat Jul 21, 2012 11:54 am

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by waltmck »

Hambalala

By how, I'm not sure if you mean for what purpose am I using it, or what is the specific coding I'm using.

For the purpose, the Mahoney and Madgwick filters are used to smooth out the sensor data in a dynamic environment. After calibrating the sensors, I was able to copy the roll and pitch algorithms out of the Adafruit libraries and I got quite accurate roll and pitch (down to less than 0.5 degree) in a static environment. But if I then put some lateral acceleration, the values got real noisy.

The Mahoney filter has smoothed things out quite a bit, I can put lateral acceleration and still get a fairly smooth value. How smooth depends on how I set the Kp gain factor for the algorithm. If it set it low, I only get a small amount of noise but it takes lower for the filter to converge on the correct angle. If I set it higher, it converges on the angle quicker but has more noise.

For the how, I copied the C code for both the Madgwick and Mahoney filters from the web site about the Madgwick algorithm (http://www.x-io.co.uk/open-source-imu-a ... lgorithms/ . I only copied the .c files and not the .h files. I put the .c file in a new tab in the sketch where I'm reading the sensors. I commented out the header file in his .c code and moved the quaternion and sample frequency variable to my sensor sketch. Then I just call his routine using my calibrated sensor values.

It is important that your sensor values are calibrated. I did a pretty good calibration of mine, but I still needed to adjust them slightly after I started calling the algorithm. Things I've seen say that the gyro readings need to be in rad/sec, but that acceleration and mag values are unit neutral. I haven't found that to be true for acceleration. I've needed to convert the acceleration from the m/s^2 that the library gives back to milliGs to get it to work right. This may be due to my not quite understanding things and how to use the algorithm. I've tried a few sampling rates and settled on 25 Hz. I've heard that it works well down to 10 Hz.

The Madgwick filter seems to work better in that I get less noise than I get from the Mahoney filter under dynamic load. But so far, I haven't got it to give me the correct angle value. I'm getting about two-thirds what the angle should be. For instance, if I incline it to 45 degrees, I get about 30 degrees as a result. This is consistent enough that it feels like a calibration error or error in my calculations that I haven't found yet.

I'm also running my gyro readings through a running-median filter before sending them to the Mahoney algorithm. This is because I'm getting an intermittent spike on my yAxis reading from the sensor. It happens for just one reading and then goes away and it happens at random intervals. The running-median filter removes these.

The filters give you a quaternion as a result, so I then use the equations that convert this into Euler angles for roll, pitch and yaw.

I haven't been paying much attention to yaw as it's not important to me. The last time I looked, I was getting drift in it, but I haven't looked since adjusting my calibration values.

User avatar
Hambalala
 
Posts: 9
Joined: Sun Feb 08, 2015 2:56 pm

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by Hambalala »

Thanks for your suggestions! Is it possible for you to share your code so that it can help me make the process easier?

User avatar
waltmck
 
Posts: 25
Joined: Sat Jul 21, 2012 11:54 am

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by waltmck »

This isn't my main program. I'm combining the sensors with a GPS logger for that. This is a program that I used to test the calculations. I got one of Adafruit's smallpan/tilt servo setups and mounted the sensors on it. The picture doesn't show it mounted. I've taken it off to do my final packaging. I had to nip the wings on the pan/tilt so the sensor board would snap in.
20150410_164502.jpg
20150410_164502.jpg (483.86 KiB) Viewed 2228 times
I used the pan/tilt to move the sensor through +/- 20 degrees in roll and pitch then printed out the results. It does show the calculations. I'm getting fair results but haven't played with the parameters. I've got to the point where "good enough and the enemy of better" on this project. it will do what I really need for this summer, but it's not as good as I would like.

What I've done, separately, is run the pan/tilt without any calculations and just logged the raw values and the servo readings for twenty minutes. At some point I'll probably come back and process that data off line.

The sketch for the pan/tilt has three tabs. The main code tab, a tab for the Mahoney code and a tab for the Madgwick code.

The main code is:

Code: Select all

//This sketch is setup to run on an Arduino MEGA
//
//Will move the sensor through about + and - 20 degree pitch and roll
//and report out the angles
//
#include <SPI.h>
#include <Wire.h>
#include <Servo.h> 
Servo servoX;  
Servo servoY;  

#include <Adafruit_10DOF.h>      // Adafruit wrapper for the 10 DOF board
#include <Adafruit_Sensor.h>     // Adafruit unifed sensor wrapper
#include <Adafruit_LSM303_U.h>   // Adafruit LSM303 accelerometer and magnometer
#include <Adafruit_BMP085_U.h>   // Adafruit pressure and tempeture sensor
#include <Adafruit_L3GD20_U.h>   // Adafruit L3GD20 gyro
Adafruit_10DOF                dof   = Adafruit_10DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_BMP085_Unified       bmp   = Adafruit_BMP085_Unified(18001);
Adafruit_L3GD20_Unified       gyro  = Adafruit_L3GD20_Unified(20);

#include <avr/sleep.h>           // Needed for timer interrupt

#include <RunningMedian.h>
RunningMedian xGyroMedianArray = RunningMedian(5);
RunningMedian yGyroMedianArray = RunningMedian(5);
RunningMedian zGyroMedianArray = RunningMedian(5);
RunningMedian xAccelMedianArray = RunningMedian(10);
RunningMedian yAccelMedianArray = RunningMedian(10);
RunningMedian zAccelMedianArray = RunningMedian(10);

// Set the pins used
const int slaveSelect = 53;
const int chipSelect = 10;

volatile unsigned int millisCnt = 0;

int servoPinX = 44;
int servoPinY = 45;

int servoAngleY = 0;      // servo position in degrees 
int servo0Y = 85;         // assumed level value 
int servoMinY = 65;       // lowest angle to go to 
int servoMaxY = 105;      // highest angle to go to 
int servoTimerTestY = 5;  // numbers of interrupts to count before change 
int servoTimerCntY = 0;   // number of triggers

int servoAngleX = 0;      // servo position in degrees 
int servo0X = 100;        // assumed level value 
int servoMinX = 80;       // lowest angle to go to 
int servoMaxX = 120;      // highest angle to go to 
int servoTimerTestX = 2; // number of triggers to count before change 
int servoTimerCntX = 0;   // number of triggers

boolean servoXDirectionPositive = true;
boolean servoYDirectionPositive = true;

// Calibration values for accel
float accelFactor = 100.937;               // Convert gravity in m/sec^2 to milliGs
float xAccelScale = 1.003;                 // My calculated xAxis scale
float xAccelOffset = 0.095;                   // My calculated xAxis offset
float yAccelScale = 0.979;                 // My calculated yAxis scale
float yAccelOffset = 0.47;                    // My calculated yAxis offset
float zAccelScale = 0.982;                 // My calculated zAxis scale
float zAccelOffset = -0.615;                  // My calculated zAxis offset

// Calibration values for gyro
float xGyroOffset  = -0.0102;
float yGyroOffset  = -0.0403;
float zGyroOffset  = -0.0748;
float xGyroScale   = 1.0069;
float yGyroScale   = 1.0241;
float zGyroScale   = 0.9959;

// Calibration values for magnetometer
float xMagOffset  = -2.59;
float yMagOffset  = 3.135;
float zMagOffset  = 0.765;
float xMagScale  = 1;
float yMagScale  = 0.9806;
float zMagScale  = 0.9474;

// Sensor raw readings
float xAccelRaw = 0.0;                            // Reported xAxis value
float yAccelRaw = 0.0;                            // Reported yAxis value
float zAccelRaw = 0.0;                            // Reported zAxis value
float xGyroRaw = 0.0;
float yGyroRaw = 0.0;
float zGyroRaw = 0.0;
float xMagRaw = 0.0;
float yMagRaw = 0.0;
float zMagRaw = 0.0;

// Accel calibrated values
float xAccelCal = 0.0;                     // My calibated xAxix value
float yAccelCal = 0.0;                     // My calibrated yAxis value
float zAccelCal = 0.0;                     // My calibrated zAxis value
float xAccelMedian  = 0.0;
float yAccelMedian  = 0.0;
float zAccelMedian  = 0.0;
float xGyroCal  = 0.0;
float yGyroCal  = 0.0;
float zGyroCal  = 0.0;
float xGyroMedian  = 0.0;
float yGyroMedian  = 0.0;
float zGyroMedian  = 0.0;
float xMagCal  = 0.0;
float yMagCal  = 0.0;
float zMagCal  = 0.0;


float unfilteredRoll  = 0.0;
float unfilteredPitch = 0.0;
float intermediateValue = 0;
float unfilteredYaw   = 0.0;

float madgwickRoll  = 0.0;
float madgwickPitch = 0.0;
float madgwickYaw   = 0.0;
float mahoneyRoll  = 0.0;
float mahoneyPitch = 0.0;
float mahoneyYaw   = 0.0;

float radToDegree = 57.2957795;          // Factor to convet radians to degrees
float knotsToMPH = 1.15078;

volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame
volatile float mahq0 = 1.0f, mahq1 = 0.0f, mahq2 = 0.0f, mahq3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame
// #define sampleFreq	512.0f		// sample frequency in Hz
// #define sampleFreq	25.0f		// sample frequency in Hz
float sampleFreq = 20.0;
float betaDef	 = 0.1f;		// 2 * proportional gain

void setup() {
  servoX.attach(servoPinX); 
  servoY.attach(servoPinY); 
  Serial.begin(9600);

  // Set to servos to level
  servoAngleX = servo0X;
  servoAngleY = servo0Y;
  servoX.write(servoAngleX);               
  servoY.write(servoAngleY); 
  
/* Initialise the sensors */
  initSensors();
  setupMakerPlot();
  
  // Set the Timer 2 register values
  cli();                       // Disable global interrupts
  TCCR2A = 0;                  // Set Timer Control 2 rgister to 0
  TCCR2B = 0;                  // Set other half of Timer Control 2 register to 0
  TCNT2 = 0;                   // Initialize counter value to 0
  
  OCR2A = 249;                  // Set COMPA for Timer 2 to 249
  TCCR2B |= (1 << CS22);        // Start with 64 prescale
  
  TCCR2A |= (1 << WGM21);      // Turn on CTC mode
  TIMSK2 |= (1 << OCIE2A);     // Enable timer compare interrupt for 2A
  sei();                       // Enable global interrupts
  
}

// Timer 2 COMPA interrupt routine
// Interrupt is called once a millisecond
SIGNAL(TIMER2_COMPA_vect) {
  ++millisCnt;
}

void loop() {
  // Check the sensor read timer
  if (millisCnt >= 49 ) {      // It's time to read them
     readSensors();
     processSensors();
     millisCnt = 0;
     ++servoTimerCntX;
     ++servoTimerCntY;
  }
  
  if (servoTimerCntX >= servoTimerTestX) {
    if (servoXDirectionPositive == true) {
      ++servoAngleX;
      if (servoAngleX >= servoMaxX) servoXDirectionPositive = false;
    }
    else {
      --servoAngleX;
      if (servoAngleX <= servoMinX) servoXDirectionPositive = true;
    }
    servoTimerCntX = 0;
    servoX.write(servoAngleX);               
  }
  
  if (servoTimerCntY >= servoTimerTestY) {
    if (servoYDirectionPositive == true) {
      ++servoAngleY;
      if (servoAngleY >= servoMaxY) servoYDirectionPositive = false;
    }
    else {
      --servoAngleY;
      if (servoAngleY <= servoMinY) servoYDirectionPositive = true;
    }
    servoTimerCntY = 0;
    servoY.write(servoAngleY);               
  }
  
  printAngles();
  
  
}


void initSensors()
{
  if(!accel.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
  Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
    while(1);
  }
  if(!mag.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
  Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
    while(1);
  }
  if(!bmp.begin())
  {
    /* There was a problem detecting the BMP180 ... check your connections */
  Serial.println(F("Ooops, no BMP180 detected ... Check your wiring!"));
    while(1);
  }
  if(!gyro.begin())
  {
    /* There was a problem detecting the L3GD20 ... check your connections */
    Serial.print(F("Ooops, no L3GD20 detected ... Check your wiring or I2C ADDR!"));
    while(1);
  }
}
void readSensors() {
  // Start Adafruit library specific reads
  sensors_event_t event;
  
  // Get the accelerometer data
  accel.getEvent(&event);
  xAccelRaw = event.acceleration.x;
  yAccelRaw = event.acceleration.y;
  zAccelRaw = event.acceleration.z;
  
  // Get the gyro data
  gyro.getEvent(&event);
  xGyroRaw = event.gyro.x;
  yGyroRaw = event.gyro.y;
  zGyroRaw = event.gyro.z;
  
  // Get the magnetometer data
  mag.getEvent(&event);
  xMagRaw = event.magnetic.x;
  yMagRaw = event.magnetic.y;
  zMagRaw = event.magnetic.z;
}

void processSensors(){
  // Do the sensor calibration
  xAccelCal = (xAccelRaw + xAccelOffset) * xAccelScale * accelFactor;
  yAccelCal = (yAccelRaw + yAccelOffset) * yAccelScale * accelFactor;
  zAccelCal = (zAccelRaw + zAccelOffset) * zAccelScale * accelFactor;
  xAccelMedianArray.add(xAccelCal);
  xAccelMedian = xAccelMedianArray.getMedian();
  yAccelMedianArray.add(yAccelCal);
  yAccelMedian = yAccelMedianArray.getMedian();
  zAccelMedianArray.add(zAccelCal);
  zAccelMedian = zAccelMedianArray.getMedian();
 
  xGyroCal = (xGyroRaw + xGyroOffset) * xGyroScale;
  yGyroCal = (yGyroRaw + yGyroOffset) * yGyroScale;
  zGyroCal = (zGyroRaw + zGyroOffset) * zGyroScale;
  xGyroMedianArray.add(xGyroCal);
  xGyroMedian = xGyroMedianArray.getMedian();
  yGyroMedianArray.add(yGyroCal);
  yGyroMedian = yGyroMedianArray.getMedian();
  zGyroMedianArray.add(zGyroCal);
  zGyroMedian = zGyroMedianArray.getMedian();
  
  xMagCal = (xMagRaw + xMagOffset) * xMagScale;
  yMagCal = (yMagRaw + yMagOffset) * yMagScale;
  zMagCal = (zMagRaw + zMagOffset) * zMagScale;
  
  // Perform the calculations with the Mahoney algorithm
  MahonyAHRSupdate(xGyroMedian, yGyroMedian, zGyroMedian, 
               xAccelMedian, yAccelMedian, zAccelMedian,
               xMagCal, yMagCal, zMagCal); 
  mahoneyRoll = atan2(2*(mahq0*mahq1+mahq2*mahq3),(1-2*(mahq1*mahq1+mahq2*mahq2))); 
  mahoneyPitch = asin(2*(mahq0*mahq2-mahq3*mahq1));
  mahoneyYaw = atan2(2*(mahq0*mahq3+mahq1*mahq2), 1-2*(mahq2*mahq2+mahq3*mahq3));
  mahoneyRoll *= radToDegree;
  mahoneyPitch *= radToDegree;
  mahoneyYaw *= radToDegree;
  
  // Perform the calculations with the Madgwick algorithm
  MadgwickAHRSupdate(xGyroMedian, yGyroMedian, zGyroMedian, 
               xAccelMedian, yAccelMedian, zAccelMedian,
               xMagCal, yMagCal, zMagCal); 
  madgwickRoll = atan2(2*(q0*q1+q2*q3),(1-2*(q1*q1+q2*q2))); 
  madgwickPitch = asin(2*(q0*q2-q3*q1));
  madgwickYaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
  madgwickRoll *= radToDegree;
  madgwickPitch *= radToDegree;
  madgwickYaw *= radToDegree;
  
  
  // Calculate roll and pith from the accelerometer values
//  accelRoll = atan2(yAccelCal, zAccelCal)*180/PI;         
//  intermediateValue = yAccelCal*sin(accelRoll + zAccelCal*cos(accelRoll));
//  accelPitch = atan(-xAccelCal/intermediateValue*180/PI); 
//  unfilteredYaw = atan2(yMagCal, xMagCal) * 180 / PI;

  // Another formula for roll and pitch from accelerometer
  intermediateValue = sqrt(xAccelMedian * xAccelMedian + zAccelMedian*zAccelMedian);
  unfilteredRoll = atan2(yAccelMedian, intermediateValue)*180/PI; 
  intermediateValue = sqrt(yAccelMedian * yAccelMedian + zAccelMedian*zAccelMedian);
  unfilteredPitch = atan2(xAccelMedian, intermediateValue)*180/PI; 
  intermediateValue = xMagCal * cos(unfilteredPitch);
  intermediateValue += yMagCal * sin(unfilteredPitch) * sin(unfilteredRoll); 
  intermediateValue += zMagCal * sin(unfilteredPitch) * cos(unfilteredRoll);
  unfilteredYaw = atan2(zMagCal * sin(unfilteredRoll) - yMagCal * cos(unfilteredRoll), intermediateValue);
}
 void setupMakerPlot() {
  delay(100);
  Serial.println(F("!O txt_Group2=Pitch"));  // Label txt_Group2
  Serial.println(F("!O txt_Group2=Pitch"));  // Label txt_Group2
  Serial.println(F("!O txt_Group1=Roll")); // Label txt_Group1
  
  Serial.println(F("!O txt_Channel0=Madgwick Roll"));  // Label txt_Channel0
  Serial.println(F("!O txt_Channel1=Mahoney Roll"));  // Label txt_Channel1
  Serial.println(F("!O txt_Channel2=Unfiltered Roll"));  // Label txt_Channel2
  Serial.println(F("!O txt_Channel3=Servo X Angle"));  // Label txt_Channel0
  Serial.println(F("!O txt_Channel4=Not used"));  // Label txt_Channel1
  Serial.println(F("!O txt_Channel5=Madgwick Pitch"));  // Label txt_Channel2
  Serial.println(F("!O txt_Channel6=Mahoney Pitch"));  // Label txt_Channel0
  Serial.println(F("!O txt_Channel7=Unfiltered Pitch"));  // Label txt_Channel1
  Serial.println(F("!O txt_Channel8=Servo Y Angle"));  // Label txt_Channel2
  Serial.println(F("!O txt_Channel9=Not used"));  // Label txt_Channel2
 }   
 
 void printAngles() {
  Serial.print(madgwickRoll);
  Serial.print(",");
  Serial.print(mahoneyRoll);
  Serial.print(",");
  Serial.print(unfilteredRoll);
  Serial.print(",");
  Serial.print(-(servoAngleX - servo0X));
  Serial.print(",");
  Serial.print(0.0, 1);
  Serial.print(",");
  Serial.print(-madgwickPitch);
  Serial.print(",");
  Serial.print(-mahoneyPitch);
  Serial.print(",");
  Serial.print(unfilteredPitch);
  Serial.print(",");
  Serial.print(servoAngleY- servo0Y);
  Serial.print(",");
  Serial.println(0.0, 1);
}
The Madgwick code is:

Code: Select all

//=====================================================================================================
// Madgwick.cpp
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author          Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
// 03/03/2015	Walt McKinnon   Converted to Arduino library
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

// #include "MadgwickAHRS.h"
#include <math.h>

//---------------------------------------------------------------------------------------------------
// Definitions

// #define sampleFreq	512.0f		// sample frequency in Hz
// #define sampleFreq	25.0f		// sample frequency in Hz
// #define betaDef	0.1f		// 2 * proportional gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float beta = betaDef;								// 2 * proportional gain (Kp)
// volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt(float x);

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
void getQuaternion(float q0Out, float q1Out, float q2Out, float q3Out) {
  q0Out = q0;
  q1Out = q0;
  q2Out = q0;
  q3Out = q0;
}

// AHRS algorithm update

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
	float recipNorm;
	float s0, s1, s2, s3;
	float qDot1, qDot2, qDot3, qDot4;
	float hx, hy;
	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;

	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
	if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
		MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
		return;
	}

	// 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);

	// 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 * (1.0f / sampleFreq);
	q1 += qDot2 * (1.0f / sampleFreq);
	q2 += qDot3 * (1.0f / sampleFreq);
	q3 += qDot4 * (1.0f / sampleFreq);

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

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
	float recipNorm;
	float s0, s1, s2, s3;
	float qDot1, qDot2, qDot3, qDot4;
	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

	// 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);

	// 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 * (1.0f / sampleFreq);
	q1 += qDot2 * (1.0f / sampleFreq);
	q2 += qDot3 * (1.0f / sampleFreq);
	q3 += qDot4 * (1.0f / sampleFreq);

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

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float invSqrt(float x) {
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i >> 1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

//====================================================================================================
// END OF CODE
//====================================================================================================
The Mahoney code is:

Code: Select all

//=====================================================================================================
// MahonyAHRS.c
//=====================================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author			Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

// #include "MahonyAHRS.h"
#include <math.h>

//---------------------------------------------------------------------------------------------------
// Definitions

// #define sampleFreq	512.0f			// sample frequency in Hz
//#define twoKpDef	(2.0f * 0.5f)	// 2 * proportional gain

//#define twoKpDef	(2.0f * 1.5f)	// 2 * proportional gain
//#define twoKiDef	(2.0f * 0.0f)	// 2 * integral gain

//#define twoKpDef	(2.0f * 1.5f)	// 2 * proportional gain
//#define twoKiDef	(2.0f * 0.1f)	// 2 * integral gain

#define twoKpDef	(2.0f * 1.5f)	// 2 * proportional gain
#define twoKiDef	(2.0f * 0.2f)	// 2 * integral gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float twoKp = twoKpDef;											// 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef;											// 2 * integral gain (Ki)
// volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrtMah(float x);

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
	float recipNorm;
    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
	float hx, hy, bx, bz;
	float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
	float halfex, halfey, halfez;
	float qa, qb, qc;

	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
		MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
		return;
	}

	// 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 = invSqrtMah(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;     

		// Normalise magnetometer measurement
		recipNorm = invSqrtMah(mx * mx + my * my + mz * mz);
		mx *= recipNorm;
		my *= recipNorm;
		mz *= recipNorm;   

        // Auxiliary variables to avoid repeated arithmetic
        q0q0 = mahq0 * mahq0;
        q0q1 = mahq0 * mahq1;
        q0q2 = mahq0 * mahq2;
        q0q3 = mahq0 * mahq3;
        q1q1 = mahq1 * mahq1;
        q1q2 = mahq1 * mahq2;
        q1q3 = mahq1 * mahq3;
        q2q2 = mahq2 * mahq2;
        q2q3 = mahq2 * mahq3;
        q3q3 = mahq3 * mahq3;   

        // Reference direction of Earth's magnetic field
        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
        bx = sqrt(hx * hx + hy * hy);
        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

		// Estimated direction of gravity and magnetic field
		halfvx = q1q3 - q0q2;
		halfvy = q0q1 + q2q3;
		halfvz = q0q0 - 0.5f + q3q3;
        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
	
		// Error is sum of cross product between estimated direction and measured direction of field vectors
		halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
		halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
		halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

		// Compute and apply integral feedback if enabled
		if(twoKi > 0.0f) {
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// Apply proportional feedback
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	
	// Integrate rate of change of quaternion
	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	qa = mahq0;
	qb = mahq1;
	qc = mahq2;
	mahq0 += (-qb * gx - qc * gy - mahq3 * gz);
	mahq1 += (qa * gx + qc * gz - mahq3 * gy);
	mahq2 += (qa * gy - qb * gz + mahq3 * gx);
	mahq3 += (qa * gz + qb * gy - qc * gx); 
	
	// Normalise quaternion
	recipNorm = invSqrtMah(mahq0 * mahq0 + mahq1 * mahq1 + mahq2 * mahq2 + mahq3 * mahq3);
	mahq0 *= recipNorm;
	mahq1 *= recipNorm;
	mahq2 *= recipNorm;
	mahq3 *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
	float recipNorm;
	float halfvx, halfvy, halfvz;
	float halfex, halfey, halfez;
	float qa, qb, qc;

	// 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 = invSqrtMah(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;        

		// Estimated direction of gravity and vector perpendicular to magnetic flux
		halfvx = mahq1 * mahq3 - mahq0 * mahq2;
		halfvy = mahq0 * mahq1 + mahq2 * mahq3;
		halfvz = mahq0 * mahq0 - 0.5f + mahq3 * mahq3;
	
		// Error is sum of cross product between estimated and measured direction of gravity
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);

		// Compute and apply integral feedback if enabled
		if(twoKi > 0.0f) {
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// Apply proportional feedback
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	
	// Integrate rate of change of quaternion
	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	qa = mahq0;
	qb = mahq1;
	qc = mahq2;
	mahq0 += (-qb * gx - qc * gy - mahq3 * gz);
	mahq1 += (qa * gx + qc * gz - mahq3 * gy);
	mahq2 += (qa * gy - qb * gz + mahq3 * gx);
	mahq3 += (qa * gz + qb * gy - qc * gx); 
	
	// Normalise quaternion
	recipNorm = invSqrtMah(mahq0 * mahq0 + mahq1 * mahq1 + mahq2 * mahq2 + mahq3 * mahq3);
	mahq0 *= recipNorm;
	mahq1 *= recipNorm;
	mahq2 *= recipNorm;
	mahq3 *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float invSqrtMah(float x) {
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

User avatar
Hambalala
 
Posts: 9
Joined: Sun Feb 08, 2015 2:56 pm

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by Hambalala »

Something that I tried recently was this code below. Although it does not include the tab procedure you used, I guess that should not make a difference yet unless there are mistakes in following the coding functions. However, I seem to get not any angle at all.

Code: Select all

//---------------------------------------------------------------------------------------------------
// Libraries
#include <math.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_10DOF.h>
#include <Adafruit_Simple_AHRS.h>

//---------------------------------------------------------------------------------------------------
// Sensor instances
Adafruit_LSM303_Accel_Unified accel(30301);
Adafruit_LSM303_Mag_Unified   mag(30302);
Adafruit_BMP085_Unified       bmp(18001);
Adafruit_L3GD20_Unified       gyro  = Adafruit_L3GD20_Unified(20);

//---------------------------------------------------------------------------------------------------
// Definitions

#define sampleFreq   25.0f      // sample frequency in Hz
#define betaDef      3.0f      // 2 * proportional gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float beta = betaDef;                        // 2 * proportional gain (Kp)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;   // quaternion of sensor frame relative to auxiliary frame

//---------------------------------------------------------------------------------------------------
// Function declarations
float invSqrt(float x);
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x) {
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;
  i = 0x5f3759df - (i >> 1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));
  return y;
}

int psi, theta, phi;
void setup() {
  Serial.begin(9600);
  Serial.println(F("Adafruit 10DOF Tester"));
  Serial.println("");

  accel.begin();
  mag.begin();
  bmp.begin();
  gyro.begin();
}

void loop() {
  MadgwickAHRSupdate();
  psi = atan2((2 * q1 * q2) - (2 * q0 * q3), (2 * pow(q0, 2)) + (2 * pow(q1, 2)) - 1);
  theta = asin((2 * q1 * q3) + (2 * q0 * q2));
  phi = atan2((2 * q2 * q3) - (2 * q0 * q1), (2 * pow(q0, 2)) + (2 * pow(q3, 2)) - 1);
Serial.print(psi);
  Serial.print(",");
  Serial.print(theta);
  Serial.print(",");
  Serial.println(phi);
  delay(500);
}

void MadgwickAHRSupdate() {
  /* Get a new sensor event */
  sensors_event_t eventg;
  gyro.getEvent(&eventg);
  float  gx = eventg.gyro.x;
  float  gy = eventg.gyro.y;
  float  gz = eventg.gyro.z;

  /* Get a new sensor event */
  sensors_event_t eventa;
  accel.getEvent(&eventa);
  float  ax = (eventa.acceleration.x)*101;
  float  ay = eventa.acceleration.y*101;
  float  az = eventa.acceleration.z*101;
  /* Get a new sensor event */
  sensors_event_t eventm;
  mag.getEvent(&eventm);
  float  mx = eventm.magnetic.x;
  float  my = eventm.magnetic.y;
  float  mz = eventm.magnetic.z;
  float recipNorm;
  float s0, s1, s2, s3;
  float qDot1, qDot2, qDot3, qDot4;
  float hx, hy;
  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;

  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
    MadgwickAHRSupdateIMU();
    return;
  }

  // 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);

  // 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 * (1.0f / sampleFreq);
  q1 += qDot2 * (1.0f / sampleFreq);
  q2 += qDot3 * (1.0f / sampleFreq);
  q3 += qDot4 * (1.0f / sampleFreq);

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

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MadgwickAHRSupdateIMU() {
  /* Get a new sensor event */
  sensors_event_t eventg;
  gyro.getEvent(&eventg);
  float  gx = eventg.gyro.x;
  float  gy = eventg.gyro.y;
  float  gz = eventg.gyro.z;

  /* Get a new sensor event */
  sensors_event_t eventa;
  accel.getEvent(&eventa);
  float  ax = eventa.acceleration.x;
  float  ay = eventa.acceleration.y;
  float  az = eventa.acceleration.z;
  /* Get a new sensor event */
  sensors_event_t eventm;
  mag.getEvent(&eventm);
  float  mx = eventm.magnetic.x;
  float  my = eventm.magnetic.y;
  float  mz = eventm.magnetic.z;
  float recipNorm;
  float s0, s1, s2, s3;
  float qDot1, qDot2, qDot3, qDot4;
  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 , _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

  // 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);

  // 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 * (1.0f / sampleFreq);
  q1 += qDot2 * (1.0f / sampleFreq);
  q2 += qDot3 * (1.0f / sampleFreq);
  q3 += qDot4 * (1.0f / sampleFreq);

  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}
Anyways, I am gonna try your one removing the unnecessary part. A question that I always had was how the sample frequency in the code makes a difference in the reading. and How different is the value that you achieve? Are you satisfied with what you get?
P.S. I really want to solve this issue for once and all.

User avatar
waltmck
 
Posts: 25
Joined: Sat Jul 21, 2012 11:54 am

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by waltmck »

I'm really not happy with the results of the Madgwick algorithm yet. I'm not sure if its because of the sample frequency or because I don't understand the parameters enough yet. If you look at the paper for the Madgwick algorithm, it has a graph that shows it's effectiveness verses sample frequency. If I'm remembering right, you need a sample frequency of over 200 per second before you start getting out of the knee on the graph. I can't even get close to that on the Arduino. The Mahoney algorithm follows the servo angles better and does smooth out much of the noise, but I'm still not satisfied with it either. It is still too responsive to noise in the accelerometers. I suspect that this is because I'm still favoring them over the gyro too much. I haven't played with the parameters enough to see what I would need to do with them to try to favor the gyro more.

I plan on playing with this offline with the data I've collected, and even trying other custom things that I've been thing off. Like calculating based on the gyro, but resetting to the accelerometer when it looks stable. But I probably won't be doing this for awhile. I mostly want the GPS logging function and then I want to measure pitch and roll of a ship on the few times with do we a large event. I'm going to be able to do this with this code, so I've moved on to a couple of other projects. When I finish them. I'll probably move back to working on these algorithms.

User avatar
Malzar
 
Posts: 1
Joined: Wed Jun 03, 2015 2:40 pm

Re: Problem in reading for AHRS of Adafruit 10 DOF IMU

Post by Malzar »

I'm also having a hard time calibrating my sensor. I'm using an Arduino Uno with your Adafruit 10-DOF IMU Breakout. I'm using Unity3d to render a cube that mirrors the sensors' output. The orientation coming from the sensor seems perfect until you try to rotate the board along a single axis too much. There seems to be a pretty severe heading error.

All sensor data is sent to my computer through an Ethernet shield and is calibrated via Unity3d's native language, C#. Seeing as how the above referenced URL includes Madgwick's code written in C#, I just grabbed a copy of that.

Just a heads up as to why the axes seem strangely ordered in my code...
Traditionally...
x = roll
y = pitch
z = heading

Unity3d...
x = pitch
y = heading
z = roll

MadgwickAHRS.cs

Code: Select all

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace AHRS
{
    /// <summary>
    /// MadgwickAHRS class. Implementation of Madgwick's IMU and AHRS algorithms.
    /// </summary>
    /// <remarks>
    /// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
    /// </remarks>
	public class MadgwickAHRS
    {
        /// <summary>
        /// Gets or sets the sample period.
        /// </summary>
        public float SamplePeriod { get; set; }

        /// <summary>
        /// Gets or sets the algorithm gain beta.
        /// </summary>
        public float Beta { get; set; }

        /// <summary>
        /// Gets or sets the Quaternion output.
        /// </summary>
        public float[] Quaternion { get; set; }

        /// <summary>
        /// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
        /// </summary>
        /// <param name="samplePeriod">
        /// Sample period.
        /// </param>
        public MadgwickAHRS(float samplePeriod)
            : this(samplePeriod, 1f)
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
        /// </summary>
        /// <param name="samplePeriod">
		/// Sample period.
        /// </param>
        /// <param name="beta">
        /// Algorithm gain beta.
        /// </param>
        public MadgwickAHRS(float samplePeriod, float beta)
        {
            SamplePeriod = samplePeriod;
            Beta = beta;
            Quaternion = new float[] { 1f, 0f, 0f, 0f };
        }

        /// <summary>
        /// Algorithm AHRS update method. Requires only gyroscope and accelerometer data.
        /// </summary>
        /// <param name="gx">
        /// Gyroscope x axis measurement in radians/s.
        /// </param>
        /// <param name="gy">
        /// Gyroscope y axis measurement in radians/s.
        /// </param>
        /// <param name="gz">
        /// Gyroscope z axis measurement in radians/s.
        /// </param>
        /// <param name="ax">
        /// Accelerometer x axis measurement in any calibrated units.
        /// </param>
        /// <param name="ay">
        /// Accelerometer y axis measurement in any calibrated units.
        /// </param>
        /// <param name="az">
        /// Accelerometer z axis measurement in any calibrated units.
        /// </param>
        /// <param name="mx">
        /// Magnetometer x axis measurement in any calibrated units.
        /// </param>
        /// <param name="my">
        /// Magnetometer y axis measurement in any calibrated units.
        /// </param>
        /// <param name="mz">
        /// Magnetometer z axis measurement in any calibrated units.
        /// </param>
        /// <remarks>
        /// Optimised for minimal arithmetic.
        /// Total ±: 160
        /// Total *: 172
        /// Total /: 5
        /// Total sqrt: 5
        /// </remarks> 
        public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
        {
            float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3];   // short name local variable for readability
            float norm;
            float hx, hy, _2bx, _2bz;
            float s1, s2, s3, s4;
            float qDot1, qDot2, qDot3, qDot4;

            // Auxiliary variables to avoid repeated arithmetic
            float _2q1mx;
            float _2q1my;
            float _2q1mz;
            float _2q2mx;
            float _4bx;
            float _4bz;
            float _2q1 = 2f * q1;
            float _2q2 = 2f * q2;
            float _2q3 = 2f * q3;
            float _2q4 = 2f * q4;
            float _2q1q3 = 2f * q1 * q3;
            float _2q3q4 = 2f * q3 * q4;
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
            if (norm == 0f) return; // handle NaN
            norm = 1 / norm;        // use reciprocal for division
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = (float)Math.Sqrt(mx * mx + my * my + mz * mz);
            if (norm == 0f) return; // handle NaN
            norm = 1 / norm;        // use reciprocal for division
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            _2q1mx = 2f * q1 * mx;
            _2q1my = 2f * q1 * my;
            _2q1mz = 2f * q1 * mz;
            _2q2mx = 2f * q2 * mx;
            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            _2bx = (float)Math.Sqrt(hx * hx + hy * hy);
            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
            _4bx = 2f * _2bx;
            _4bz = 2f * _2bz;

            // Gradient decent algorithm corrective step
            s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            norm = 1f / (float)Math.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
            s1 *= norm;
            s2 *= norm;
            s3 *= norm;
            s4 *= norm;

            // Compute rate of change of quaternion
            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;

            // Integrate to yield quaternion
            q1 += qDot1 * SamplePeriod;
            q2 += qDot2 * SamplePeriod;
            q3 += qDot3 * SamplePeriod;
            q4 += qDot4 * SamplePeriod;
            norm = 1f / (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
            Quaternion[0] = q1 * norm;
            Quaternion[1] = q2 * norm;
            Quaternion[2] = q3 * norm;
            Quaternion[3] = q4 * norm;
        }

        /// <summary>
        /// Algorithm IMU update method. Requires only gyroscope and accelerometer data.
        /// </summary>
        /// <param name="gx">
        /// Gyroscope x axis measurement in radians/s.
        /// </param>
        /// <param name="gy">
        /// Gyroscope y axis measurement in radians/s.
        /// </param>
        /// <param name="gz">
        /// Gyroscope z axis measurement in radians/s.
        /// </param>
        /// <param name="ax">
        /// Accelerometer x axis measurement in any calibrated units.
        /// </param>
        /// <param name="ay">
        /// Accelerometer y axis measurement in any calibrated units.
        /// </param>
        /// <param name="az">
        /// Accelerometer z axis measurement in any calibrated units.
        /// </param>
        /// <remarks>
        /// Optimised for minimal arithmetic.
        /// Total ±: 45
        /// Total *: 85
        /// Total /: 3
        /// Total sqrt: 3
        /// </remarks>
        public void Update(float gx, float gy, float gz, float ax, float ay, float az)
        {
            float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3];   // short name local variable for readability
            float norm;
            float s1, s2, s3, s4;
            float qDot1, qDot2, qDot3, qDot4;

            // Auxiliary variables to avoid repeated arithmetic
            float _2q1 = 2f * q1;
            float _2q2 = 2f * q2;
            float _2q3 = 2f * q3;
            float _2q4 = 2f * q4;
            float _4q1 = 4f * q1;
            float _4q2 = 4f * q2;
            float _4q3 = 4f * q3;
            float _8q2 = 8f * q2;
            float _8q3 = 8f * q3;
            float q1q1 = q1 * q1;
            float q2q2 = q2 * q2;
            float q3q3 = q3 * q3;
            float q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
            if (norm == 0f) return; // handle NaN
            norm = 1 / norm;        // use reciprocal for division
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Gradient decent algorithm corrective step
            s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
            s2 = _4q2 * q4q4 - _2q4 * ax + 4f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
            s3 = 4f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
            s4 = 4f * q2q2 * q4 - _2q2 * ax + 4f * q3q3 * q4 - _2q3 * ay;
            norm = 1f / (float)Math.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
            s1 *= norm;
            s2 *= norm;
            s3 *= norm;
            s4 *= norm;

            // Compute rate of change of quaternion
            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;

            // Integrate to yield quaternion
            q1 += qDot1 * SamplePeriod;
            q2 += qDot2 * SamplePeriod;
            q3 += qDot3 * SamplePeriod;
            q4 += qDot4 * SamplePeriod;
            norm = 1f / (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
            Quaternion[0] = q1 * norm;
            Quaternion[1] = q2 * norm;
            Quaternion[2] = q3 * norm;
            Quaternion[3] = q4 * norm;
        }
    }
}
Code used to render the cube in Unity3d...

Code: Select all

using UnityEngine;
using System.Collections;
using System;

public class SensorExample : MonoBehaviour 
{
	AHRS.MadgwickAHRS calibration;

	Transform leftCube;

	Orientation orientation;
	Acceleration acceleration;
	Torque torque;



	void Start () 
	{
		calibration = new AHRS.MadgwickAHRS (25.0f);
	}
	



	void Update () 
	{
		//-------------------------------------------------------------------
		//CODE GOES HERE THAT UPDATES TORQUE(GYRO), ORIENTATION, AND ACCELERATION.
		//-------------------------------------------------------------------

		//Left cube reflects raw data.
		leftCube.eulerAngles = new Vector3(-orientation.pitch, -orientation.heading, -orientation.roll);
		
		calibration.Update(-deg2Rad(torque.x), 
		                   -deg2Rad(torque.y), 
		                   -deg2Rad(torque.z), 
		                   -m2G(acceleration.x), 
		                   -m2G(acceleration.y), 
		                   -m2G(acceleration.z), 
		                   -orientation.pitch, 
		                   -orientation.heading, 
		                   -orientation.roll);

		float[] quat = calibration.Quaternion;
		this.transform.rotation = new Quaternion(quat[0], quat[1], quat[2], quat[3]);
	}




	
	//meters per second squared conversion to milli g-units
	public float m2G(float m)
	{
		//1 m/s^2 = 0.10197162129779 g-units
		//1 g-unit = 1000 millig-units
		return (m * 0.10197162129779f) * 1000;
	}
	
	//degrees to radians conversion...
	public float deg2Rad(float d)
	{
		return (float)(d * Math.PI) / 180.0f;
	}




	
	public struct Torque
	{
		public float x;
		public float y;
		public float z;
		
		public Torque(float x, float y, float z)
		{
			this.x = x;
			this.y = y;
			this.z = z;
		}
	}
	
	public struct Acceleration
	{
		public float x;
		public float y;
		public float z;
		
		public Acceleration(float x, float y, float z)
		{
			this.x = x;
			this.y = y;
			this.z = z;
		}
	}
	public struct Orientation
	{
		public float roll;
		public float pitch;
		public float heading;
		
		public Orientation(float roll, float pitch, float heading)
		{
			this.roll = roll;
			this.pitch = pitch;
			this.heading = heading;
		}
	}
}

Video of results...
Left Cube: Uncalibrated. (heading error)
Right Cube: Madgwick calibrated. (broken)
Granted, I'm rotating it by hand, but the heading error is a little more obvious at the end of the video when I rotate it around 180 degrees.
https://youtu.be/eqRDyFNqr3U

Anyone ever figure out how to implement Madgwick's code properly?

Locked
Please be positive and constructive with your questions and comments.

Return to “Other Products from Adafruit”