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

Refactored arbitrary gyro and mag alignment.

Browse source 
The original implementation:

* removed the old 'alignment' variable
* did not require 'ALIGN_CUSTOM'
* always used rotation matrix
* had no additional per-pid-loop conditional logic.

Extract currently unused code into tests.
In preparation for either deleting or re-using in validateAndFixConfig.

Fix code style of some old boardalignment code.

De-duplicate vector rotation code.

Now that rotation code is exacted from `alignBoard` and now  doesn't use
`boardRotation` some if it was similar to the code in `rotateV` in
maths.c

Use DECIDEGREES for mag and gyro/acc custom alignments.

Use unnamed structure instead of `values`.

Redefine what 'custom' orientation means.

Move alignment test-only code into the tests.

Ensure gyro/mag custom alignment settings follow the enum variations.

This can't be applied to ALIGN_DEFAULT because, in the case of the MAG,
the default isn't actually known until the gyro is detected, see
`compassDetect`.

OMNIBUSF4/F7 - Don't use ALIGN_DEFAULT in target.h,
common_defaults_post.h does this now.

Comment cleanup.

Delete unused alignment code left from various tests/refactoring
efforts.

* Please do not squash this commit.

Fix SITL build by avoiding structure assignment with anonymous inner
struct.

The error from the build server was as follows:

```./src/main/common/sensor_alignment.c:49:5: error: missing initializer
for field ‘yaw’ of ‘struct <anonymous>’
[-Werror=missing-field-initializers]
     *sensorAlignment = CUSTOM_ALIGN_CW0_DEG;
     ^
In file included from ./src/main/common/sensor_alignment.c:27:0:
./src/main/common/sensor_alignment.h:80:17: note: ‘yaw’ declared here
         int16_t yaw;
                 ^
```

Cleanup sensor_alignment API.
This commit is contained in:
Dominic Clifton 2019-06-26 00:12:51 +02:00 • committed by mikeller
parent 494b559277
commit 980df1536f
27 changed files with 558 additions and 139 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -23,6 +23,8 @@
#include "platform.h"
#include "build/build_config.h"
#include "axis.h"
#include "maths.h"
@ -197,7 +199,7 @@ void normalizeV(struct fp_vector *src, struct fp_vector *dest)
}
}
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
{
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
@ -214,15 +216,25 @@ void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
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;
rotation->m[0][X] = cosz * cosy;
rotation->m[0][Y] = -cosy * sinz;
rotation->m[0][Z] = siny;
rotation->m[1][X] = sinzcosx + (coszsinx * siny);
rotation->m[1][Y] = coszcosx - (sinzsinx * siny);
rotation->m[1][Z] = -sinx * cosy;
rotation->m[2][X] = (sinzsinx) - (coszcosx * siny);
rotation->m[2][Y] = (coszsinx) + (sinzcosx * siny);
rotation->m[2][Z] = cosy * cosx;
}
FAST_CODE void applyRotation(float *v, fp_rotationMatrix_t *rotationMatrix)
{
struct fp_vector *vDest = (struct fp_vector *)v;
struct fp_vector vTmp = *vDest;
vDest->X = (rotationMatrix->m[0][X] * vTmp.X + rotationMatrix->m[1][X] * vTmp.Y + rotationMatrix->m[2][X] * vTmp.Z);
vDest->Y = (rotationMatrix->m[0][Y] * vTmp.X + rotationMatrix->m[1][Y] * vTmp.Y + rotationMatrix->m[2][Y] * 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.
@ -230,13 +242,11 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
{
struct fp_vector v_tmp = *v;
float matrix[3][3];
fp_rotationMatrix_t rotationMatrix;
buildRotationMatrix(delta, matrix);
buildRotationMatrix(delta, &rotationMatrix);
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];
applyRotation((float *)&v_tmp, &rotationMatrix);
}
// Quick median filter implementation