Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Moved vectors, euler angles, rotation and normalization from imu to maths.

Browse source
This commit is contained in:
Pierre Hugo 2015-01-21 19:42:06 -08:00
parent 919a8796e7
commit 29c5630957
4 changed files with 78 additions and 71 deletions
Download patch file Download diff file Expand all files Collapse all files

49
src/main/flight/imu.c
View file

@ -145,55 +145,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
t_fp_vector EstG;
// 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;
}
}
// 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 mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta->angles.roll);
sinx = sinf(delta->angles.roll);
cosy = cosf(delta->angles.pitch);
siny = sinf(delta->angles.pitch);
cosz = cosf(delta->angles.yaw);
sinz = sinf(delta->angles.yaw);
coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;
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];
}
void accSum_reset(void)
{
accSum[0] = 0;