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

Cleaned up matrix rotation.

Browse source
This commit is contained in:
mikeller 2021-04-26 23:10:00 +12:00
parent 76b6415d34
commit 55cf26ebbc
4 changed files with 5 additions and 56 deletions
Download patch file Download diff file Expand all files Collapse all files

27
src/main/common/maths.c
View file

@ -186,19 +186,6 @@ float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float des
return (a / b) + destFrom;
}
// Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
{
float length;
length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
if (length != 0) {
dest->X = src->X / length;
dest->Y = src->Y / length;
dest->Z = src->Z / length;
}
}
void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
{
float cosx, sinx, cosy, siny, cosz, sinz;
@ -227,7 +214,7 @@ void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
rotation->m[2][Z] = cosy * cosx;
}
FAST_CODE void applyRotation(float *v, fp_rotationMatrix_t *rotationMatrix)
void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix)
{
struct fp_vector *vDest = (struct fp_vector *)v;
struct fp_vector vTmp = *vDest;
@ -237,18 +224,6 @@ FAST_CODE void applyRotation(float *v, fp_rotationMatrix_t *rotationMatrix)
vDest->Z = (rotationMatrix->m[0][Z] * vTmp.X + rotationMatrix->m[1][Z] * vTmp.Y + rotationMatrix->m[2][Z] * vTmp.Z);
}
// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
void rotateV(struct fp_vector *v, fp_angles_t *delta)
{
struct fp_vector v_tmp = *v;
fp_rotationMatrix_t rotationMatrix;
buildRotationMatrix(delta, &rotationMatrix);
applyRotation((float *)&v_tmp, &rotationMatrix);
}
// Quick median filter implementation
// (c) N. Devillard - 1998
// http://ndevilla.free.fr/median/median.pdf