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

Merge remote-tracking branch 'multiwii/master' into

Browse source 
project-structure-alternative

Conflicts:
	src/sensors.c
This commit is contained in:
Dominic Clifton 2014-04-16 17:49:10 +01:00
commit 3c1ba729b9
5 changed files with 30 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/flight_imu.c
View file

@ -269,10 +269,12 @@ static void getEstimatedAttitude(void)
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG))
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, deltaGyroAngle);
else
} else {
rotateV(&EstN.V, deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V);
}
// 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.
@ -311,11 +313,11 @@ static void getEstimatedAttitude(void)
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
throttleAngleCorrection = 0;
} else {
} else {
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;
throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / 900.0f * M_PI / 2.0f)) ;
throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ;
}
}
@ -352,7 +354,7 @@ int getEstimatedAltitude(void)
if (calibratingB > 0) {
baroGroundPressure -= baroGroundPressure / 8;
baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1);
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
vel = 0;
accAlt = 0;