mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 00:35:39 +03:00
removed retarded small_angles shit and replaced with proper calculation.
no more small angles while shaking the board. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@415 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
6763d8810b
commit
91d64fc6c1
2 changed files with 8 additions and 7 deletions
14
src/imu.c
14
src/imu.c
|
@ -5,7 +5,7 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
|
|||
int32_t accSum[3];
|
||||
uint32_t accTimeSum = 0; // keep track for integration of acc
|
||||
int accSumCount = 0;
|
||||
int16_t acc_25deg = 0;
|
||||
int16_t accZ_25deg = 0;
|
||||
int32_t baroPressure = 0;
|
||||
int32_t baroTemperature = 0;
|
||||
uint32_t baroPressureSum = 0;
|
||||
|
@ -32,7 +32,7 @@ static void getEstimatedAttitude(void);
|
|||
|
||||
void imuInit(void)
|
||||
{
|
||||
acc_25deg = acc_1G * 0.423f;
|
||||
accZ_25deg = acc_1G * cosf(RAD * 25.0f);
|
||||
accVelScale = 9.80665f / acc_1G / 10000.0f;
|
||||
|
||||
#ifdef MAG
|
||||
|
@ -253,11 +253,6 @@ static void getEstimatedAttitude(void)
|
|||
if (sensors(SENSOR_MAG))
|
||||
rotateV(&EstM.V, deltaGyroAngle);
|
||||
|
||||
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
|
||||
|
@ -271,6 +266,11 @@ static void getEstimatedAttitude(void)
|
|||
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
|
||||
}
|
||||
|
||||
if (abs(EstG.A[Z]) > accZ_25deg)
|
||||
f.SMALL_ANGLES_25 = 1;
|
||||
else
|
||||
f.SMALL_ANGLES_25 = 0;
|
||||
|
||||
// Attitude of the estimated vector
|
||||
anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
|
||||
anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue