Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 09:45:37 +03:00
Code Issues Wiki Activity

Merge pull request #10712 from mikeller/cleanup_matrix_rotation

Browse source 
Cleaned up matrix rotation.
This commit is contained in:
Michael Keller 2021-05-18 00:46:04 +12:00 • committed by GitHub
commit dfcd11bd69
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
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

@ -179,19 +179,6 @@ float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float des
return (a / b) + destFrom; 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) void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
{ {
float cosx, sinx, cosy, siny, cosz, sinz; float cosx, sinx, cosy, siny, cosz, sinz;
@ -220,7 +207,7 @@ void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
rotation->m[2][Z] = cosy * cosx; 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 *vDest = (struct fp_vector *)v;
struct fp_vector vTmp = *vDest; struct fp_vector vTmp = *vDest;
@ -230,18 +217,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); 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 // Quick median filter implementation
// (c) N. Devillard - 1998 // (c) N. Devillard - 1998
// http://ndevilla.free.fr/median/median.pdf // http://ndevilla.free.fr/median/median.pdf

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

@ -113,11 +113,8 @@ float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo); int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);
float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float 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 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 quickMedianFilter3(int32_t * v);
int32_t quickMedianFilter5(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); 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) FAST_CODE_NOINLINE void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* sensorRotationMatrix)
{ {
applyRotation(dest, sensorRotationMatrix); applyMatrixRotation(dest, sensorRotationMatrix);
if (!standardBoardAlignment) { if (!standardBoardAlignment) {
alignBoard(dest); 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); 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) #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
TEST(MathsUnittest, TestFastTrigonometrySinCos) TEST(MathsUnittest, TestFastTrigonometrySinCos)
{ {