Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

merged changes from multiwii_dev 20120504. this means new serial protocol, new buzzer code

Browse source 
fixed spacing in ledring.c
defaulted acc_lpf to 100
correction in vtail4 mix (from multiwii_dev)
trashed more unused LOG_VALUES crap
no binary build since this is untested / non-flight-tested.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@152 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-05-05 12:47:52 +00:00
parent 910df63a7f
commit 0d7460960e
13 changed files with 694 additions and 172 deletions
Download patch file Download diff file Expand all files Collapse all files

15
src/imu.c
View file

@ -4,6 +4,7 @@
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
int16_t acc_25deg = 0;
int32_t BaroAlt;
int16_t sonarAlt; //to think about the unit
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
@ -175,7 +176,7 @@ static void getEstimatedAttitude(void)
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
static float accTemp[3]; // projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
float scale, deltaGyroAngle[3];
@ -187,10 +188,8 @@ static void getEstimatedAttitude(void)
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale;
if (cfg.acc_lpf_factor > 0) {
accTemp[axis] = accTemp[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
accSmooth[axis] = roundf(accTemp[axis]);
// accTemp[axis] = (accTemp[axis] - (accTemp[axis] >> cfg.acc_lpf_factor)) + accADC[axis];
// accSmooth[axis] = accTemp[axis] >> cfg.acc_lpf_factor;
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
accSmooth[axis] = accLPF[axis];
} else {
accSmooth[axis] = accADC[axis];
}
@ -221,10 +220,8 @@ static void getEstimatedAttitude(void)
// 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) || smallAngle25) {
for (axis = 0; axis < 3; axis++) {
// int16_t acc = accSmooth[axis];
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accTemp[axis]) * INV_GYR_CMPF_FACTOR;
}
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
if (sensors(SENSOR_MAG)) {