rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-1355 Fix Mag scaling being calculated after applying board rotation.

Browse Source
This commit is contained in:
Alessio Morale 2014-05-25 11:28:43 +02:00
parent 4581481852
commit 3d67b7567e
2 changed files with 24 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
flight/libraries/inc/CoordinateConversions.h
View File

@ -75,5 +75,27 @@ void quat_inverse(float q[4]);
void quat_copy(const float q[4], float qnew[4]);
void quat_mult(const float q1[4], const float q2[4], float qout[4]);
void rot_mult(float R[3][3], const float vec[3], float vec_out[3]);
/**
* matrix_mult_3x3f - perform a multiplication between two 3x3 float matrices
* result = a*b
* @param a
* @param b
* @param result
*/
inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
{
result[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2];
result[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2];
result[0][2] = a[0][2] * b[0][0] + a[1][2] * b[0][1] + a[2][2] * b[0][2];
result[1][0] = a[0][0] * b[1][0] + a[1][0] * b[1][1] + a[2][0] * b[1][2];
result[1][1] = a[0][1] * b[1][0] + a[1][1] * b[1][1] + a[2][1] * b[1][2];
result[1][2] = a[0][2] * b[1][0] + a[1][2] * b[1][1] + a[2][2] * b[1][2];
result[2][0] = a[0][0] * b[2][0] + a[1][0] * b[2][1] + a[2][0] * b[2][2];
result[2][1] = a[0][1] * b[2][0] + a[1][1] * b[2][1] + a[2][1] * b[2][2];
result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
}
#endif // COORDINATECONVERSIONS_H_

31
flight/modules/Sensors/sensors.c
View File

@ -62,6 +62,7 @@
#include <CoordinateConversions.h>
#include <pios_board_info.h>
#include <pios_struct_helper.h>
// Private constants
#define STACK_SIZE_BYTES 1000
@ -474,35 +475,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
mag_transform[0][0] = R[0][0] * cal.mag_transform.r0c0 +
R[1][0] * cal.mag_transform.r0c1 +
R[2][0] * cal.mag_transform.r0c2;
mag_transform[0][1] = R[0][1] * cal.mag_transform.r0c0 +
R[1][1] * cal.mag_transform.r0c1 +
R[2][1] * cal.mag_transform.r0c2;
mag_transform[0][2] = R[0][2] * cal.mag_transform.r0c0 +
R[1][2] * cal.mag_transform.r0c1 +
R[2][2] * cal.mag_transform.r0c2;
mag_transform[1][0] = R[0][0] * cal.mag_transform.r1c0 +
R[1][0] * cal.mag_transform.r1c1 +
R[2][0] * cal.mag_transform.r1c2;
mag_transform[1][1] = R[0][1] * cal.mag_transform.r1c0 +
R[1][1] * cal.mag_transform.r1c1 +
R[2][1] * cal.mag_transform.r1c2;
mag_transform[1][2] = R[0][2] * cal.mag_transform.r1c0 +
R[1][2] * cal.mag_transform.r1c1 +
R[2][2] * cal.mag_transform.r1c2;
mag_transform[1][0] = R[0][0] * cal.mag_transform.r2c0 +
R[1][0] * cal.mag_transform.r2c1 +
R[2][0] * cal.mag_transform.r2c2;
mag_transform[2][1] = R[0][1] * cal.mag_transform.r2c0 +
R[1][1] * cal.mag_transform.r2c1 +
R[2][1] * cal.mag_transform.r2c2;
mag_transform[2][2] = R[0][2] * cal.mag_transform.r2c0 +
R[1][2] * cal.mag_transform.r2c1 +
R[2][2] * cal.mag_transform.r2c2;
matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform);
}
/**
* @}