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

remove unnecessary variable. unroll loop for better code size.

Browse source
This commit is contained in:
Dominic Clifton 2014-06-18 23:42:43 +01:00
parent fd59a4cd52
commit f96bdf0965
1 changed files with 6 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/flight/imu.c
View file

@ -102,7 +102,6 @@ void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *ini
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration) void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration)
{ {
uint32_t axis;
static int16_t gyroYawSmooth = 0; static int16_t gyroYawSmooth = 0;
gyroGetADC(); gyroGetADC();
@ -115,14 +114,14 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfigurat
accADC[Z] = 0; accADC[Z] = 0;
} }
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
if (mixerConfiguration == MULTITYPE_TRI) { if (mixerConfiguration == MULTITYPE_TRI) {
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3; gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
gyroYawSmooth = gyroData[FD_YAW]; gyroYawSmooth = gyroData[FD_YAW];
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
} else { } else {
for (axis = 0; axis < 3; axis++) gyroData[FD_YAW] = gyroADC[FD_YAW];
gyroData[axis] = gyroADC[axis];
} }
} }
@ -164,7 +163,7 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
// This does a "proper" matrix rotation using gyro deltas without small-angle approximation // This does a "proper" matrix rotation using gyro deltas without small-angle approximation
float mat[3][3]; float mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz; float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx; float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta->angles.roll); cosx = cosf(delta->angles.roll);
sinx = sinf(delta->angles.roll); sinx = sinf(delta->angles.roll);
@ -174,12 +173,11 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
sinz = sinf(delta->angles.yaw); sinz = sinf(delta->angles.yaw);
coszcosx = cosz * cosx; coszcosx = cosz * cosx;
coszcosy = cosz * cosy;
sinzcosx = sinz * cosx; sinzcosx = sinz * cosx;
coszsinx = sinx * cosz; coszsinx = sinx * cosz;
sinzsinx = sinx * sinz; sinzsinx = sinx * sinz;
mat[0][0] = coszcosy; mat[0][0] = cosz * cosy;
mat[0][1] = -cosy * sinz; mat[0][1] = -cosy * sinz;
mat[0][2] = siny; mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny); mat[1][0] = sinzcosx + (coszsinx * siny);