rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

higer precision float radian angles are now used for acc and mag rotation 

no need to cripple the readings here

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@402 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-09-15 12:29:29 +00:00
parent 26db228f79
commit 6d3467c759
1 changed files with 12 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/imu.c
View File

@ -26,6 +26,7 @@ float accVelScale;
int16_t gyroData[3] = { 0, 0, 0 }; int16_t gyroData[3] = { 0, 0, 0 };
int16_t gyroZero[3] = { 0, 0, 0 }; int16_t gyroZero[3] = { 0, 0, 0 };
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
float anglerad[2] = { 0, 0 }; // absolute angle inclination in radians
static void getEstimatedAttitude(void); static void getEstimatedAttitude(void);
@ -183,12 +184,6 @@ void rotateV(struct fp_vector *v, float *delta)
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
} }
static int16_t _atan2f(float y, float x)
{
// no need for aidsy inaccurate shortcuts on a proper platform
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
}
int32_t applyDeadband(int32_t value, int32_t deadband) int32_t applyDeadband(int32_t value, int32_t deadband)
{ {
if (abs(value) < deadband) { if (abs(value) < deadband) {
@ -209,9 +204,9 @@ void acc_calc(uint32_t deltaT)
t_fp_vector accel_ned; t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame // the accel values have to be rotated into the earth frame
rpy[0] = -(float) angle[ROLL] * RADX10; rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float) angle[PITCH] * RADX10; rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float) heading * RADX10 * 10; rpy[2] = -(float)heading * RADX10 * 10.0f;
accel_ned.V.X = accSmooth[0]; accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1]; accel_ned.V.Y = accSmooth[1];
@ -301,21 +296,21 @@ static void getEstimatedAttitude(void)
} }
// Attitude of the estimated vector // Attitude of the estimated vector
angle[ROLL] = _atan2f(EstG.V.Y, EstG.V.Z); anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
angle[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));
angle[ROLL] = anglerad[ROLL] * (1800.0f / M_PI); // roll angle in 0.1 deg steps
angle[PITCH] = anglerad[PITCH] * (1800.0f / M_PI); // pitch angle in 0.1 deg steps
#ifdef MAG #ifdef MAG
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
// baseflight calculation (no, sorry, tarducopter calculation) // baseflight calculation (no, sorry, tarducopter calculation)
float rollRAD = (float)angle[ROLL] * RADX10;
float pitchRAD = (float)angle[PITCH] * RADX10;
float magX = EstM.A[X]; float magX = EstM.A[X];
float magY = EstM.A[Y]; float magY = EstM.A[Y];
float magZ = EstM.A[Z]; float magZ = EstM.A[Z];
float cr = cosf(rollRAD); float cr = cosf(anglerad[ROLL]);
float sr = sinf(rollRAD); float sr = sinf(anglerad[ROLL]);
float cp = cosf(pitchRAD); float cp = cosf(anglerad[PITCH]);
float sp = sinf(pitchRAD); float sp = sinf(anglerad[PITCH]);
float Xh = magX * cp + magY * sr * sp + magZ * cr * sp; float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
float Yh = magY * cr - magZ * sr; float Yh = magY * cr - magZ * sr;
float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f; float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;