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

removed retarded small_angles shit and replaced with proper calculation.

Browse source 
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:
timecop@gmail.com 2013-09-29 13:10:58 +00:00
parent 6763d8810b
commit 91d64fc6c1
2 changed files with 8 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/board.h
View file

@ -28,6 +28,7 @@
#endif /* M_PI */ #endif /* M_PI */
#define RADX10 (M_PI / 1800.0f) // 0.001745329252f #define RADX10 (M_PI / 1800.0f) // 0.001745329252f
#define RAD (M_PI / 180.0f)
#define min(a, b) ((a) < (b) ? (a) : (b)) #define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b)) #define max(a, b) ((a) > (b) ? (a) : (b))

14
src/imu.c
View file

@ -5,7 +5,7 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
int32_t accSum[3]; int32_t accSum[3];
uint32_t accTimeSum = 0; // keep track for integration of acc uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0; int accSumCount = 0;
int16_t acc_25deg = 0; int16_t accZ_25deg = 0;
int32_t baroPressure = 0; int32_t baroPressure = 0;
int32_t baroTemperature = 0; int32_t baroTemperature = 0;
uint32_t baroPressureSum = 0; uint32_t baroPressureSum = 0;
@ -32,7 +32,7 @@ static void getEstimatedAttitude(void);
void imuInit(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; accVelScale = 9.80665f / acc_1G / 10000.0f;
#ifdef MAG #ifdef MAG
@ -253,11 +253,6 @@ static void getEstimatedAttitude(void)
if (sensors(SENSOR_MAG)) if (sensors(SENSOR_MAG))
rotateV(&EstM.V, deltaGyroAngle); 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) // 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. // 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 // 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; 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 // Attitude of the estimated vector
anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z); 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)); anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));