Commit 4fcecbd6 by Dominic Clifton

De-duplicate vector matrix calculation code.

`Saves 248 bytes of flash space when using -O0.`
parent 6ce64d8e
 ... ... @@ -18,6 +18,7 @@ #include #include #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 ... ...
 ... ... @@ -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); ... ...
 ... ... @@ -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) ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!