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

Merge pull request #12 from luggi/rotationfailfix 

fixed rotation fail
This commit is contained in:
dongie 2013-11-06 03:03:31 -08:00
parent c14e7cf59f 2d029105a5
commit 14f087a140
1 changed files with 29 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/utils.c
View File

@ -27,6 +27,8 @@ float constrainf(float amt, float low, float high)
void initBoardAlignment(void)
{
float roll, pitch, yaw;
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
// standard alignment, nothing to calculate
if (!mcfg.board_align_roll && !mcfg.board_align_pitch && !mcfg.board_align_yaw)
@ -38,19 +40,32 @@ void initBoardAlignment(void)
roll = mcfg.board_align_roll * M_PI / 180.0f;
pitch = mcfg.board_align_pitch * M_PI / 180.0f;
yaw = mcfg.board_align_yaw * M_PI / 180.0f;
cosx = cosf(roll);
sinx = sinf(roll);
cosy = cosf(pitch);
siny = sinf(pitch);
cosz = cosf(yaw);
sinz = sinf(yaw);
coszcosx = cosz * cosx;
coszcosy = cosz * cosy;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
// define rotation matrix
boardRotation[0][0] = cosf(roll) * cosf(pitch);
boardRotation[0][1] = cosf(roll) * sinf(pitch) * sinf(yaw) - sinf(roll) * cosf(yaw);
boardRotation[0][2] = cosf(roll) * sinf(pitch) * cosf(yaw) + sinf(roll) * sinf(yaw);
boardRotation[1][0] = sinf(roll) * cosf(pitch);
boardRotation[1][1] = sinf(roll) * sinf(pitch) * sinf(yaw) + cosf(roll) * cosf(yaw);
boardRotation[1][2] = sinf(roll) * sinf(pitch) * cosf(yaw) - cosf(roll) * sinf(yaw);
boardRotation[2][0] = -sinf(pitch);
boardRotation[2][1] = cosf(pitch) * sinf(yaw);
boardRotation[2][2] = cosf(pitch) * cosf(yaw);
boardRotation[0][0] = coszcosy;
boardRotation[0][1] = -cosy * sinz;
boardRotation[0][2] = siny;
boardRotation[1][0] = sinzcosx + (coszsinx * siny);
boardRotation[1][1] = coszcosx - (sinzsinx * siny);
boardRotation[1][2] = -sinx * cosy;
boardRotation[2][0] = (sinzsinx) - (coszcosx * siny);
boardRotation[2][1] = (coszsinx) + (sinzcosx * siny);
boardRotation[2][2] = cosy * cosx;
}
void alignBoard(int16_t *vec)
@ -59,9 +74,9 @@ void alignBoard(int16_t *vec)
int16_t y = vec[Y];
int16_t z = vec[Z];
vec[X] = boardRotation[0][0] * x + boardRotation[0][1] * y + boardRotation[0][2] * z;
vec[Y] = boardRotation[1][0] * x + boardRotation[1][1] * y + boardRotation[1][2] * z;
vec[Z] = boardRotation[2][0] * x + boardRotation[2][1] * y + boardRotation[2][2] * z;
vec[X] = lrintf(boardRotation[0][0] * x + boardRotation[1][0] * y + boardRotation[2][0] * z);
vec[Y] = lrintf(boardRotation[0][1] * x + boardRotation[1][1] * y + boardRotation[2][1] * z);
vec[Z] = lrintf(boardRotation[0][2] * x + boardRotation[1][2] * y + boardRotation[2][2] * z);
}
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)