360 degree operation with gyroscope and accelerometer

First off, let me make the point that I didn't write this program. My Arduino skills are limited...

I hired an EE to help me develop this project and overall he did an excellent job, but got stuck on trying to make this rotate 360 degrees, saying that it was a too difficult maths problem to get past each 90 degree boundary.

The project is a self levelling platform that uses input from a gyroscope and an accelerometer to keep a stage level. The motor is a stepper. Absolute positioning is not used, only relative.

Quick video:

http://www.youtube.com/watch?v=PJ1qGxhhmhQ

Basically I want to have the base rotate 360 degrees+ and have the platform stay level. Due to the fusion filter currently being used, it gets to about 85 degrees and flips back. This video is the best explanation:

http://www.youtube.com/watch?v=1Q-K_RY4398

My main question is - how difficult a math/programming problem is this to solve? Has anyone here made it work?

My main question is - how difficult a math/programming problem is this to solve? Has anyone here made it work?

That would depend on how the code is written, wouldn't it? Post the code.

robrob: My main question is - how difficult a math/programming problem is this to solve? Has anyone here made it work?

Getting it to work at all is the hardest part. If it's a general solution I wouldn't expect it to have any problem coping with deflections greater than 90 degrees and the fact it doesn't currently cope may mean that a cruder solution has been used - for example, only using a single accelerometer axis to detect orientation.

If it's any consolation, I've read that the Arduplane project already has a 'gimbal' mode which can be used stabilise a camera platform in a given orientation wrt the real world. It's definitely feasible.

Looking a little closer, it looks like he used the library from FreeIMU:

Can this be modified, or try a different library?

I just found it has it’s own forum - Ill try ask over there.

/*
FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino
Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net>

*/

//#include <inttypes.h>
#include <stdint.h>

#include "main.h"
#include "FreeIMU.h"
#include "MPU6050.h"

//#include "vector_math.h"
/**
 * Initialize the FreeIMU, sensors and performs gyro offsets calibration
*/
void FreeIMU_init(void) {
  // disable internal pullups of the ATMEGA which Wire enable by default
  #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
    // deactivate internal pull-ups for twi
    // as per note from atmega8 manual pg167
    cbi(PORTC, 4);
    cbi(PORTC, 5);
  #endif
	// initialize quaternion
	q0 = 1.0f;
	q1 = 0.0f;
	q2 = 0.0f;
	q3 = 0.0f;
	exInt = 0.0;
	eyInt = 0.0;
	ezInt = 0.0;
	twoKp = twoKpDef;
	twoKi = twoKiDef;
	integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;

    gyro_off_x=0; gyro_off_y=0; gyro_off_z=0;
    acc_off_x=0; acc_off_y=0; acc_off_z=0;

	// initialize scale factors to neutral values
	acc_scale_x = 1;
	acc_scale_y = 1;
	acc_scale_z = 1;

	mpu6050_init();
  
	//zero gyro
	delay_ms(200);
	FreeIMU_zeroGyro();
}//void FreeIMU_init(void)

/**
 * Populates raw_values with the raw_values from the sensors
*/
void FreeIMU_getRawValues(int16_t * raw_values) {
	mpu6050_read_all_axis_data(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
}

/**
 * Populates values with calibrated readings from the sensors
*/
void FreeIMU_getValues(float * values) {

	int i;
    int16_t accgyroval[6];
    mpu6050_read_all_axis_data(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
    
    // remove offsets from the gyroscope
    accgyroval[3] = accgyroval[3] - gyro_off_x;
    accgyroval[4] = accgyroval[4] - gyro_off_y;
    accgyroval[5] = accgyroval[5] - gyro_off_z;

    for(i = 0; i<6; i++) {
      if(i < 3) {
        values[i] = (float) accgyroval[i];
      }
      else {
        values[i] = ((float) accgyroval[i]) / GYRO_SENSITIVITY; // NOTE: this depends on the sensitivity chosen
      }
    }

  // remove offsets and scale accelerometer (calibration)
  values[0] = (values[0] - acc_off_x) / acc_scale_x;
  values[1] = (values[1] - acc_off_y) / acc_scale_y;
  values[2] = (values[2] - acc_off_z) / acc_scale_z;
  //printf("%05f, %05f, %05f, %05f, %05f, %05f\r\n", values[0], values[1], values[2], values[3], values[4], values[5]);
}


/**
 * Computes gyro offsets
*/
void FreeIMU_zeroGyro(void) {
	int i;
	const int totSamples = 30;
	int16_t raw[11];
	float tmpOffsets[] = {0,0,0};

	for (i = 0; i < totSamples; i++){
		FreeIMU_getRawValues(raw);
		tmpOffsets[0] += raw[3];
		tmpOffsets[1] += raw[4];
		tmpOffsets[2] += raw[5];
		delay_ms(10);
	}

	gyro_off_x = tmpOffsets[0] / totSamples;
	gyro_off_y = tmpOffsets[1] / totSamples;
	gyro_off_z = tmpOffsets[2] / totSamples;
	printf("Gyro Offset: %05d, %05d, %05d\r\n", gyro_off_x, gyro_off_y, gyro_off_z);
}


/**
 * Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
 * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
 * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
 * axis only.
 * 
 * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
*/
void FreeIMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) {

  float recipNorm;
  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
  float qa, qb, qc;

  // Auxiliary variables to avoid repeated arithmetic
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
    float halfvx, halfvy, halfvz;

Too long - sorry I had to split it into two.

// Normalise accelerometer measurement
    recipNorm = FreeIMU_invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // Estimated direction of gravity
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;
  
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex += (ay * halfvz - az * halfvy);
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);
  }

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
    // 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 = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);
  
  // Normalise quaternion
  recipNorm = FreeIMU_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}


/**
 * Populates array q with a quaternion representing the IMU orientation with respect to the Earth
 * 
 * @param q the quaternion to populate
*/
void FreeIMU_getQ(float * q) {
    float val[9];
    FreeIMU_getValues(val);
    /*
    DEBUG_PRINT(val[0]);//accell
    DEBUG_PRINT(val[1]);
    DEBUG_PRINT(val[2]);
    DEBUG_PRINT(val[3] * M_PI/180);//gyro
    DEBUG_PRINT(val[4] * M_PI/180);
    DEBUG_PRINT(val[5] * M_PI/180);
    */
    // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
    FreeIMU_AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);

    q[0] = q0;
    q[1] = q1;
    q[2] = q2;
    q[3] = q3;
}

/**
 * Returns the Euler angles in radians defined in the Aerospace sequence.
 * See Sebastian O.H. Madwick report "An efficient orientation filter for 
 * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
 * 
 * @param angles three floats array which will be populated by the Euler angles in radians
*/
void FreeIMU_getEulerRad(float * angles) {
  float q[4]; // quaternion
  FreeIMU_getQ(q);
  angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
  angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
  angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}


/**
 * Returns the Euler angles in degrees defined with the Aerospace sequence.
 * See Sebastian O.H. Madwick report "An efficient orientation filter for 
 * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
 * 
 * @param angles three floats array which will be populated by the Euler angles in degrees
*/
void FreeIMU_getEuler(float * angles) {
    FreeIMU_getEulerRad(angles);
    FreeIMU_arr3_rad_to_deg(angles);
}


/**
 * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
 * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
 * and the Earth ground plane and the IMU Y axis.
 * 
 * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
 * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
 * 
 * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians
*/
void FreeIMU_getYawPitchRollRad(float * ypr2) {
    float q[4]; // quaternion
    float gx, gy, gz; // estimated gravity direction
    FreeIMU_getQ(q);

    gx = 2 * (q[1]*q[3] - q[0]*q[2]);
    gy = 2 * (q[0]*q[1] + q[2]*q[3]);
    gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];

    //ypr2[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
    ypr2[1] = atan(gx / sqrt(gy*gy + gz*gz));
    ypr2[2] = atan(gy / sqrt(gx*gx + gz*gz));
}


/**
 * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between
 * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
 * and the Earth ground plane and the IMU Y axis.
 * 
 * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
 * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
 * 
 * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees
*/
void FreeIMU_getYawPitchRoll(float * ypr) {
    FreeIMU_getYawPitchRollRad(ypr);
    FreeIMU_arr3_rad_to_deg(ypr);
}


/**
 * Converts a 3 elements array arr of angles expressed in radians into degrees
*/
void FreeIMU_arr3_rad_to_deg(float * arr) {
  arr[0] *= 180/M_PI;
  arr[1] *= 180/M_PI;
  arr[2] *= 180/M_PI;
}


/**
 * Fast inverse square root implementation
 * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root
*/
float FreeIMU_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;
}

hy i love this work , me to i want to built a self stabilisation platform using arduino uno, 3 actuators for X,Y,Z and IMU6050, please can you send me all the code that you created?!! this is my email :aziz-qessate_mi@hotmail.fr