Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +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

5
src/main/common/maths.h
View file

@ -114,11 +114,8 @@ float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);
float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo);
void normalizeV(struct fp_vector *src, struct fp_vector *dest);
void rotateV(struct fp_vector *v, fp_angles_t *delta);
void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation);
void applyRotation(float *v, fp_rotationMatrix_t *rotationMatrix);
void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix);
int32_t quickMedianFilter3(int32_t * v);
int32_t quickMedianFilter5(int32_t * v);

6
src/main/sensors/boardalignment.c
View file

@ -64,14 +64,14 @@ void initBoardAlignment(const boardAlignment_t *boardAlignment)
buildRotationMatrix(&rotationAngles, &boardRotation);
}
static FAST_CODE void alignBoard(float *vec)
static void alignBoard(float *vec)
{
applyRotation(vec, &boardRotation);
applyMatrixRotation(vec, &boardRotation);
}
FAST_CODE_NOINLINE void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* sensorRotationMatrix)
{
applyRotation(dest, sensorRotationMatrix);
applyMatrixRotation(dest, sensorRotationMatrix);
if (!standardBoardAlignment) {
alignBoard(dest);

23
src/test/unit/maths_unittest.cc
View file

@ -202,29 +202,6 @@ void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b, float absTo
EXPECT_NEAR(a->Z, b->Z, absTol);
}
TEST(MathsUnittest, TestRotateVectorWithNoAngle)
{
fp_vector vector = {1.0f, 0.0f, 0.0f};
fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}};
rotateV(&vector, &euler_angles);
fp_vector expected_result = {1.0f, 0.0f, 0.0f};
expectVectorsAreEqual(&vector, &expected_result, 1e-5);
}
TEST(MathsUnittest, TestRotateVectorAroundAxis)
{
// Rotate a vector <1, 0, 0> around an each axis x y and z.
fp_vector vector = {1.0f, 0.0f, 0.0f};
fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}};
rotateV(&vector, &euler_angles);
fp_vector expected_result = {1.0f, 0.0f, 0.0f};
expectVectorsAreEqual(&vector, &expected_result, 1e-5);
}
#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
TEST(MathsUnittest, TestFastTrigonometrySinCos)
{