Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
branch: upstream_shared
Fetching contributors…

Cannot retrieve contributors at this time

361 lines (311 sloc) 12.514 kb
void computeIMU () {
uint8_t axis;
static int16_t gyroADCprevious[3] = {0,0,0};
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0;
//we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
//gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
//gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
#if defined(NUNCHUCK)
annexCode();
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
ACC_getADC();
getEstimatedAttitude(); // computation time must last less than one interleaving delay
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
f.NUNCHUKDATA = 1;
while(f.NUNCHUKDATA) ACC_getADC(); // For this interleaving reading, we must have a gyro update at this point (less delay)
for (axis = 0; axis < 3; axis++) {
// empirical, we take a weighted value of the current and the previous values
// /4 is to average 4 values, note: overflow is not possible for WMP gyro here
gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis])/4;
gyroADCprevious[axis] = gyroADC[axis];
}
#else
#if ACC
ACC_getADC();
getEstimatedAttitude();
#endif
#if GYRO
Gyro_getADC();
#endif
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave=micros();
annexCode();
if ((micros()-timeInterleave)>650) {
annex650_overrun_count++;
} else {
while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
}
#if GYRO
Gyro_getADC();
#endif
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis]+gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3;
gyroADCprevious[axis] = gyroADCinter[axis]/2;
if (!ACC) accADC[axis]=0;
}
#endif
#if defined(GYRO_SMOOTHING)
static int16_t gyroSmooth[3] = {0,0,0};
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+gyroData[axis]+1 ) / conf.Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
#elif defined(TRI)
static int16_t gyroYawSmooth = 0;
gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW])/3;
gyroYawSmooth = gyroData[YAW];
#endif
}
// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// **************************************************
//****** advanced users settings *******************
/* Set the Low Pass Filter factor for ACC */
/* 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 100
#endif
/* Set the Low Pass Filter factor for Magnetometer */
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
/* Comment this if you do not want filter at all.*/
#ifndef MG_LPF_FACTOR
//#define MG_LPF_FACTOR 4
#endif
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
#ifndef GYR_CMPF_FACTOR
#define GYR_CMPF_FACTOR 400.0f
#endif
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
#ifndef GYR_CMPFM_FACTOR
#define GYR_CMPFM_FACTOR 200.0f
#endif
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#if GYRO
#define GYRO_SCALE ((2380 * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result
// +-2000/sec deg scale
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
// +- 200/sec deg scale
// 1.5 is emperical, not sure what it means
// should be in rad/sec
#else
#define GYRO_SCALE (1.0f/200e6f)
// empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility
// !!!!should be adjusted to the rad/sec
#endif
// Small angle approximation
#define ssin(val) (val)
#define scos(val) 1.0f
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
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;
}
float InvSqrt (float x){
union{
int32_t i;
float f;
} conv;
conv.f = x;
conv.i = 0x5f3759df - (conv.i >> 1);
return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
int32_t isq(int32_t x){return x * x;}
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v,float* delta) {
fp_vector v_tmp = *v;
v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
}
#define ACC_LPF_FOR_VELOCITY 10
static float accLPFVel[3];
static t_fp_vector EstG;
void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
#if MAG
static t_fp_vector EstM;
#endif
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
#if defined(ACC_LPF_FACTOR)
static float accLPF[3];
#endif
static uint16_t previousT;
uint16_t currentT = micros();
float scale, deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale;
#if defined(ACC_LPF_FACTOR)
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f/ACC_LPF_FOR_VELOCITY)) + accADC[axis] * (1.0f/ACC_LPF_FOR_VELOCITY);
accSmooth[axis] = accLPF[axis];
#define ACC_VALUE accSmooth[axis]
#else
accSmooth[axis] = accADC[axis];
#define ACC_VALUE accADC[axis]
#endif
// accMag += (ACC_VALUE * 10 / (int16_t)acc_1G) * (ACC_VALUE * 10 / (int16_t)acc_1G);
accMag += (int32_t)ACC_VALUE*ACC_VALUE ;
#if MAG
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
#endif
}
accMag = accMag*100/((int32_t)acc_1G*acc_1G);
rotateV(&EstG.V,deltaGyroAngle);
#if MAG
rotateV(&EstM.V,deltaGyroAngle);
#endif
if ( abs(accSmooth[ROLL])<acc_25deg && abs(accSmooth[PITCH])<acc_25deg && accSmooth[YAW]>0) {
f.SMALL_ANGLES_25 = 1;
} else {
f.SMALL_ANGLES_25 = 0;
}
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if ( ( 36 < accMag && accMag < 196 ) || f.SMALL_ANGLES_25 )
for (axis = 0; axis < 3; axis++) {
int16_t acc = ACC_VALUE;
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;
}
#if MAG
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
#endif
// Attitude of the estimated vector
angle[ROLL] = _atan2(EstG.V.X , EstG.V.Z) ;
angle[PITCH] = _atan2(EstG.V.Y , EstG.V.Z) ;
#if MAG
// Attitude of the cross product vector GxM
heading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
if ( heading > 180) heading = heading - 360;
else if (heading < -180) heading = heading + 360;
#endif
}
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 21
#define ACC_Z_DEADBAND (acc_1G/50)
#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}
void getEstimatedAltitude(){
static uint32_t deadLine = INIT_DELAY;
static int16_t baroHistTab[BARO_TAB_SIZE];
static int8_t baroHistIdx;
static int32_t baroHigh;
if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
uint16_t dTime = currentTime - deadLine;
deadLine = currentTime;
//**** Alt. Set Point stabilization PID ****
baroHistTab[baroHistIdx] = BaroAlt/10;
baroHigh += baroHistTab[baroHistIdx];
baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
baroHistIdx++;
if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;
//EstAlt = baroHigh*10/(BARO_TAB_SIZE-1);
EstAlt = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f; // additional LPF to reduce baro noise
#ifndef SUPPRESS_BARO_ALTHOLD
//P
int16_t error = constrain(AltHold - EstAlt, -300, 300);
applyDeadband(error, 10); //remove small P parametr to reduce noise near zero position
BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
//I
errorAltitudeI += error * conf.I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
BaroPID += (errorAltitudeI / 500); //I in range +/-60
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
//int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG;
applyDeadband(accZ, ACC_Z_DEADBAND);
//debug[0] = accZ;
static float vel = 0.0f;
static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
// Integrator - velocity, cm/sec
vel+= accZ * accVelScale * dTime;
static int32_t lastBaroAlt;
float baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = EstAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
applyDeadband(baroVel, 10); // to reduce noise near zero
//debug[1] = baroVel;
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * 0.985f + baroVel * 0.015f;
//vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
//debug[2] = vel;
//D
float vel_tmp = vel;
applyDeadband(vel_tmp, 5);
vario = vel_tmp;
BaroPID -= constrain(conf.D8[PIDALT] * vel_tmp / 20, -150, 150);
//debug[3] = BaroPID;
#endif
}
Jump to Line
Something went wrong with that request. Please try again.