From 3d67b7567eeb656fd04d37283607ccc8cba4dfef Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 25 May 2014 11:28:43 +0200 Subject: [PATCH] OP-1355 Fix Mag scaling being calculated after applying board rotation. --- flight/libraries/inc/CoordinateConversions.h | 22 ++++++++++++++ flight/modules/Sensors/sensors.c | 31 ++------------------ 2 files changed, 24 insertions(+), 29 deletions(-) diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 8c5f55858..cc32cf442 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -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_ diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index c1c76ee8f..bd35945fb 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -62,6 +62,7 @@ #include #include +#include // 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); } /** * @}