Skip to content

Commit

Permalink
Some size optimisation & using smoothed acc values now (filter doesn'…
Browse files Browse the repository at this point in the history 
…t really work with noisy data) ... test flight tomorrow ;-)
  • Loading branch information
@sebastianherp
sebastianherp committed Jul 11, 2012
1 parent 151cec0 commit aa6f305
Show file tree
Hide file tree
Showing 4 changed files with 87 additions and 52 deletions.
12 changes: 9 additions & 3 deletions IMU.ino
View file
Expand Up @@ -29,7 +29,11 @@ void computeIMU () {
#else
#if ACC
ACC_getADC();
getEstimatedAttitude();
#if defined IMU_NORMAL
getEstimatedAttitude();
#elif defined IMU_EXPERIMENTAL
getEstimatedAttitudeExperimental();
#endif
#endif
#if GYRO
Gyro_getADC();
Expand Down Expand Up @@ -90,7 +94,7 @@ void computeIMU () {
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/
/* Comment this if you do not want filter at all.*/
#ifndef ACC_LPF_FACTOR
#define ACC_LPF_FACTOR 10
#define ACC_LPF_FACTOR 100
#endif

/* Set the Low Pass Filter factor for Magnetometer */
Expand All @@ -117,7 +121,7 @@ void computeIMU () {

//****** end of advanced users settings *************

#if !defined IMU_EXPERIMENTAL


#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
Expand Down Expand Up @@ -178,6 +182,8 @@ void rotateV(struct fp_vector *v,float* delta) {
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
}

#if defined IMU_NORMAL

void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
Expand Down
122 changes: 75 additions & 47 deletions IMU_EXPERIMENTAL.ino
View file
@@ -1,29 +1,46 @@

#if defined IMU_EXPERIMENTAL

#define KpACCMAG 5.0f
#define KiACCMAG 0.001f
#define GYRO_SCALE (4.0f / 16.384f * PI / 180.0f);
#define NORM_AS_FUNCTION

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;
}

int16_t _atan2(float y, float x){
#define fp_is_neg(val) ((((uint8_t*)&val)[3] & 0x80) != 0)
float z = y / x;
int16_t zi = abs(int16_t(z * 100));
int8_t y_neg = fp_is_neg(y);
if ( zi < 100 ){
if (zi > 10)
z = z / (1.0f + 0.28f * z * z);
if (fp_is_neg(x)) {
if (y_neg) z -= PI;
else z += PI;
}
} else {
z = (PI / 2.0f) - z / (z * z + 0.28f);
if (y_neg) z -= PI;
}
z *= (180.0f / PI * 10);
return z;
void normalize3(float &x, float &y, float &z) {
float norm = sqrt(x * x + y * y + z * z);
if (norm == 0.0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
x *= norm;
y *= norm;
z *= norm;
}

void getEstimatedAttitude() {
void normalize4(float &v1, float &v2, float &v3, float &v4) {
float norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3 + v4 * v4);
if (norm == 0.0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
v1 *= norm;
v2 *= norm;
v3 *= norm;
v4 *= norm;
}


void getEstimatedAttitudeExperimental() {
uint8_t axis;
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
Expand All @@ -33,8 +50,6 @@ void getEstimatedAttitude() {
#endif

static float q1 = 1, q2 = 0, q3 = 0, q4 = 0;
static float Kp = 3.0f;
static float Ki = 0.001f;
static float eInt1, eInt2, eInt3;
float norm;
float hx, hy, bx, bz;
Expand Down Expand Up @@ -68,36 +83,45 @@ void getEstimatedAttitude() {
#else
accSmooth[axis] = accADC[axis];
#define ACC_VALUE accADC[axis]
#endif
#endif
}

float gx = -gyroADC[1] * GYRO_SCALE;
float gy = gyroADC[0] * GYRO_SCALE;
float gz = -gyroADC[2] * GYRO_SCALE;

float ax = -accADC[0];
float ay = -accADC[1];
float az = accADC[2];
float ax = -accSmooth[0];
float ay = -accSmooth[1];
float az = accSmooth[2];

float mx = magADC[0];
float my = magADC[1];
float mz = -magADC[2];

// Normalise accelerometer measurement
#if defined NORM_AS_FUNCTION
float zero = 0;
normalize4(ax, ay, az, zero);
#else
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
#endif

// Normalise magnetometer measurement
#if defined NORM_AS_FUNCTION
normalize4(mx, my, mz, zero);
#else
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
#endif

// Reference direction of Earth's magnetic field
hx = 2 * mx * (0.5f - q3q3 - q4q4) + 2 * my * (q2q3 - q1q4) + 2 * mz * (q2q4 + q1q3);
Expand All @@ -112,12 +136,24 @@ void getEstimatedAttitude() {
wx = 2 * bx * (0.5f - q3q3 - q4q4) + 2 * bz * (q2q4 - q1q3);
wy = 2 * bx * (q2q3 - q1q4) + 2 * bz * (q1q2 + q3q4);
wz = 2 * bx * (q1q3 + q2q4) + 2 * bz * (0.5f - q2q2 - q3q3);

// Calculate angles from estimation
angle[ROLL] = -_atan2(vx, sqrt(vy*vy + vz*vz));
angle[PITCH] = -_atan2(vy, sqrt(vx*vx + vz*vz));
heading = _atan2(2*(q2q3 - q1q4), 2*(q1q1 + q2q2) - 1) / 10 - 90 + MAG_DECLINIATION;
if ( heading > 180) heading = heading - 360;
else if (heading < -180) heading = heading + 360;

//debug[0] = -_atan2(vx, sqrt(vy*vy + vz*vz));
//debug[1] = -_atan2(vy, sqrt(vx*vx + vz*vz));
//debug[2] = angle[ROLL];
//debug[3] = angle[PITCH];

// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
if (Ki > 0.0f)
if (KiACCMAG > 0.0f)
{
eInt1 += ex; // accumulate integral error
eInt2 += ey;
Expand All @@ -131,41 +167,33 @@ void getEstimatedAttitude() {
}

// Apply feedback terms
gx = gx + Kp * ex + Ki * eInt1;
gy = gy + Kp * ey + Ki * eInt2;
gz = gz + Kp * ez + Ki * eInt3;
gx = gx + KpACCMAG * ex + KiACCMAG * eInt1;
gy = gy + KpACCMAG * ey + KiACCMAG * eInt2;
gz = gz + KpACCMAG * ez + KiACCMAG * eInt3;

// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * dt);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * dt);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * dt);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * dt);
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (dt * 0.5f); // (dt * 0.5f) uses less space than just dt ... wtf
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (dt * 0.5f);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (dt * 0.5f);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (dt * 0.5f);

// Normalise quaternion
#if defined NORM_AS_FUNCTION
normalize4(q1, q2, q3, q4);
#else
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
q4 = q4 * norm;
#endif

debug[0] = q1 * 1000;
debug[1] = q2 * 1000;
debug[2] = q3 * 1000;
debug[3] = q4 * 1000;

vx = 2 * (q2q4 - q1q3);
vy = 2 * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;

angle[ROLL] = -_atan2(vx, sqrt(vy*vy + vz*vz));
angle[PITCH] = -_atan2(vy, sqrt(vx*vx + vz*vz));
heading = _atan2(2*q2*q3 - 2*q1*q4, 2*q1*q1 + 2*q2*q2 - 1) / 10 - 90 + MAG_DECLINIATION;
if ( heading > 180) heading = heading - 360;
else if (heading < -180) heading = heading + 360;
}



#endif // IMU_EXPERIMENTAL
2 changes: 1 addition & 1 deletion Readme.markdown
View file
Expand Up @@ -14,7 +14,7 @@ Goals
-------------------

* staying compatible with existing MultiWii verions (currently 2.1 and its release candidates)
* robust attitude estimation code (at the expense of some precious bytes, currently 3650)
* robust attitude estimation code (at the expense of some precious bytes, currently 3250 bytes)
* better altitude estimation code and altitude hold mode
* shrinking code size (binary form), so everything still fits in a Arduino Pro Mini (328P)

Expand Down
3 changes: 2 additions & 1 deletion config.h
View file
Expand Up @@ -15,7 +15,8 @@
*/


#define IMU_EXPERIMENTAL
#define IMU_EXPERIMENTAL // uncomment to use
//#define IMU_NORMAL // uncomment to use (default imu code)

/*************************************************************************************************/
/***************** ***************/
Expand Down

0 comments on commit aa6f305

Please sign in to comment.