anyone can help me to understand the code

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
Earduino
Posts: 6
Joined: Sat Oct 10, 2015 8:54 am

anyone can help me to understand the code

Post by Earduino »

the code from multiwii2.2. I am a newbee learning the multiwii code.i try to understand the code,but not.And now l do not know how to get star,i need help
[code][/code]
void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
float scale, deltaGyroAngle[3];
static uint16_t previousT;
uint16_t currentT = micros();

scale = (currentT - previousT) * GYRO_SCALE;//计算上个系统时间到这个系统时间,即周期
previousT = currentT;

/*到此进行滤波前计算常数*/

// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale; //对加速度原始数据进行简单的低通滤波

accLPF32[axis] -= accLPF32[axis]>>ACC_LPF_FACTOR;
accLPF32[axis] += accADC[axis];
accSmooth[axis] = accLPF32[axis]>>ACC_LPF_FACTOR;

accMag += (int32_t)accSmooth[axis]*accSmooth[axis] ;//对地磁原始数据进行简单的低通滤波
}
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.15G or <0.85G 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 ( 72 < accMag && accMag < 133 )
for (axis = 0; axis < 3; axis++) {
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
#if MAG
for (axis = 0; axis < 3; axis++) {
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
EstM32.A[axis] = EstM.A[axis];
}
#endif

for (axis = 0; axis < 3; axis++)
EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float

// Attitude of the estimated vector 对你的到的数据 进行反三角函数运算
int32_t sqGZ = sq(EstG32.V.Z);
int32_t sqGX = sq(EstG32.V.X);
int32_t sqGY = sq(EstG32.V.Y);
int32_t sqGX_sqGZ = sqGX + sqGZ;
float invmagXZ = InvSqrt(sqGX_sqGZ);
invG = InvSqrt(sqGX_sqGZ + sqGY);
angle[ROLL] = _atan2(EstG32.V.X , EstG32.V.Z); //得到角度
angle[PITCH] = _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ);

#if MAG
heading = _atan2(
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
EstM32.V.Y * invG * sqGX_sqGZ - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
#endif
} //进行反三角函数运算

#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define BARO_TAB_SIZE 21

#define ACC_Z_DEADBAND (acc_1G>>5) // was 40 instead of 32 now


#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}

#if BARO
uint8_t getEstimatedAltitude(){
static float baroGroundTemperatureScale,logBaroGroundPressureSum;
static uint32_t deadLine;
static int32_t baroGroundPressure;
static uint16_t previousT;
uint16_t currentT = micros();
uint16_t dTime;

dTime = currentT - previousT;
if (dTime < UPDATE_INTERVAL) return 0;
previousT = currentT;

if(calibratingB > 0) {
logBaroGroundPressureSum = log(baroPressureSum);
baroGroundTemperatureScale = (baroTemperature + 27315) * 29.271267f;
//baroGroundPressure = baroPressureSum/(BARO_TAB_SIZE - 1);
calibratingB--;
}

// pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-meg ... P_Baro.cpp
BaroAlt = (logBaroGroundPressureSum - log(baroPressureSum)) * baroGroundTemperatureScale;

EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

#if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
int16_t targetVel = constrain(AltHold - EstAlt, -100, 100);

// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
int16_t accZ = (accSmooth[ROLL] * EstG32.V.X + accSmooth[PITCH] * EstG32.V.Y + accSmooth[YAW] * EstG32.V.Z) * invG;

static int16_t accZoffset = 0; // = acc_1G*6; //58 bytes saved and convergence is fast enough to omit init
if (!f.ARMED) {
accZoffset -= accZoffset>>3;
accZoffset += accZ;
}
accZ -= accZoffset>>3;

//applyDeadband(accZ, ACC_Z_DEADBAND);

static float vel = 0.0f;
static float accVelScale = 9.80665f / 10000.0f / acc_1G ;

//I
// Integrator - velocity, cm/sec
vel += accZ * accVelScale * dTime;

static int32_t lastBaroAlt;
int16_t 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

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

int16_t error16 = targetVel - vel;
BaroPID = constrain((conf.P8[PIDALT] * error16 >>7), -200, +200);

errorAltitudeI += conf.I8[PIDALT] * error16 >>6;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
BaroPID += errorAltitudeI>>8; //I in range +/-60

BaroPID = constrain(BaroPID, -200, 200);

debug[0] = vel;
debug[1] = error16;
debug[2] = BaroPID;
debug[3] = AltHold;

/*
//D
int16_t vel_tmp = vel;
applyDeadband(vel_tmp, 5);
vario = vel_tmp;

BaroPID -= constrain(conf.D8[PIDALT] * vel_tmp >>4, -150, 150);
*/
#endif
return 1;
}
#endif //BARO
Last edited by Earduino on Thu Oct 22, 2015 9:43 am, edited 1 time in total.

Earduino
Posts: 6
Joined: Sat Oct 10, 2015 8:54 am

Re: anyone can help me to understand the code

Post by Earduino »

my Flight Control System

Flight control: MultiWii Copter
Processor: ATMega32u4
Gyro and Accelerator: MPU6050
Magnetometer: HMCL5883L
Barometer: BMP085
BLE IC: CC2541 or CC2540
Onboard 5V Regulator
Onboard 3.3V Regulator
Power Input: 3.3~4.7V
Flight Time: about 7 minutes
Communication protocol: BLE 4.0

Earduino
Posts: 6
Joined: Sat Oct 10, 2015 8:54 am

anyone can help me to understand the code

Post by Earduino »

anyone understands the complimentary filter?

Cereal_Killer
Posts: 221
Joined: Fri Mar 06, 2015 5:44 am

Re: anyone can help me to understand the code

Post by Cereal_Killer »

What specifically are you trying to accomplish? I can't read your Chinese comments (maybe, I don't even know what language that is)...

The best way for you to get help is to be as specific as possible. Please list what EXACTLY it is you're trying to change. What code changes have you already made and did it do what you expected? Lots of us understand the code, I don't think anybody understands what you're trying to make it do. I'm really not trying to be mean to you here but I don't think anyone knows what it is you're even asking as there just isn't a clear question in this thread.

Please post very specific questions in as simple of terms as possible:
What is it you're trying to do?
What changes have you made (either site line numbers or code blocks), what changes do you expect to need to make?
What did you try that didn't work? What happened when you tried those things?
What is the end goal?

I promise I'm not attacking you personally but there just isn't a question here than anyone can answer. To have the best chance of getting a solution to your issue you need to try and work with us for us to help you.

Earduino
Posts: 6
Joined: Sat Oct 10, 2015 8:54 am

anyone can help me to understand the code

Post by Earduino »

thanks for your advise. i just want to understand how the complimentary filter work in the function:get Estimated Attitude () .In other words, I can not understand how the the function:getEstimated Attitude () work. by the way, the chinese is translation of the note from the mwc 2.2. thanks for your answering

Earduino
Posts: 6
Joined: Sat Oct 10, 2015 8:54 am

anyone can help me to understand the code

Post by Earduino »

@Cereal_Killer

Post Reply