De-duplicate vector matrix calculation code.

Saves 248 bytes of flash space when using -O0.

src/main/common/maths.c

@@ -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;
-
-    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];
+    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;
+}
+
+// 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

src/main/common/maths.h

@@ -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);

src/main/sensors/boardalignment.c

@@ -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);
-
-    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;
+    fp_angles_t rotationAngles;
+    rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees);
+    rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
+    rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees);
 
-    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)