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

De-duplicate vector matrix calculation code.

Browse source 
Saves 248 bytes of flash space when using -O0.
This commit is contained in:
Dominic Clifton 2015-06-24 22:33:36 +01:00
parent 6ce64d8ef9
commit 4fcecbd667
3 changed files with 33 additions and 52 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -18,6 +18,7 @@
#include <stdint.h>
#include <math.h>
#include "axis.h"
#include "maths.h"
int32_t applyDeadband(int32_t value, int32_t deadband)
@ -105,12 +106,8 @@ void normalizeV(struct fp_vector *src, struct fp_vector *dest)
}
}
// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
void rotateV(struct fp_vector *v, fp_angles_t *delta)
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
{
struct fp_vector v_tmp = *v;
float mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
@ -126,19 +123,29 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
mat[0][0] = cosz * cosy;
mat[0][1] = -cosy * sinz;
mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny);
mat[1][1] = coszcosx - (sinzsinx * siny);
mat[1][2] = -sinx * cosy;
mat[2][0] = (sinzsinx) - (coszcosx * siny);
mat[2][1] = (coszsinx) + (sinzcosx * siny);
mat[2][2] = cosy * cosx;
matrix[0][X] = cosz * cosy;
matrix[0][Y] = -cosy * sinz;
matrix[0][Z] = siny;
matrix[1][X] = sinzcosx + (coszsinx * siny);
matrix[1][Y] = coszcosx - (sinzsinx * siny);
matrix[1][Z] = -sinx * cosy;
matrix[2][X] = (sinzsinx) - (coszcosx * siny);
matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
matrix[2][Z] = cosy * cosx;
}
v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
// 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;
float matrix[3][3];
buildRotationMatrix(delta, matrix);
v->X = v_tmp.X * matrix[0][X] + v_tmp.Y * matrix[1][X] + v_tmp.Z * matrix[2][X];
v->Y = v_tmp.X * matrix[0][Y] + v_tmp.Y * matrix[1][Y] + v_tmp.Z * matrix[2][Y];
v->Z = v_tmp.X * matrix[0][Z] + v_tmp.Y * matrix[1][Z] + v_tmp.Z * matrix[2][Z];
}
// Quick median filter implementation

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

@ -77,6 +77,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
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, float matrix[3][3]);
int32_t quickMedianFilter3(int32_t * v);
int32_t quickMedianFilter5(int32_t * v);

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

@ -37,45 +37,18 @@ static bool isBoardAlignmentStandard(boardAlignment_t *boardAlignment)
void initBoardAlignment(boardAlignment_t *boardAlignment)
{
float roll, pitch, yaw;
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
if (isBoardAlignmentStandard(boardAlignment)) {
return;
}
standardBoardAlignment = false;
roll = degreesToRadians(boardAlignment->rollDegrees);
pitch = degreesToRadians(boardAlignment->pitchDegrees);
yaw = degreesToRadians(boardAlignment->yawDegrees);
fp_angles_t rotationAngles;
rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees);
rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees);
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] = 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;
buildRotationMatrix(&rotationAngles, boardRotation);
}
static void alignBoard(int16_t *vec)
@ -84,9 +57,9 @@ static void alignBoard(int16_t *vec)
int16_t y = vec[Y];
int16_t z = vec[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);
vec[X] = lrintf(boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z);
vec[Y] = lrintf(boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z);
vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
}
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)