Paparazzi UAS  v5.15_devel-50-g4d7045d
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_dcm_algebra.h
Go to the documentation of this file.
1 /*
2  * Released under Creative Commons License
3  *
4  * 2010 The Paparazzi Team
5  *
6  *
7  * Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
8  * Version 1.0 for flat board updated by Doug Weibel and Jose Julio
9  *
10  */
11 
20 #ifndef AHRS_FLOAT_DCM_ALGEBRA_H
21 #define AHRS_FLOAT_DCM_ALGEBRA_H
22 
23 
24 static inline float Vector_Dot_Product(float vector1[3], float vector2[3])
25 {
26  return vector1[0] * vector2[0] + vector1[1] * vector2[1] + vector1[2] * vector2[2];
27 }
28 
29 static inline void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
30 {
31  vectorOut[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
32  vectorOut[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
33  vectorOut[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
34 }
35 
36 static inline void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
37 {
38  vectorOut[0] = vectorIn[0] * scale2;
39  vectorOut[1] = vectorIn[1] * scale2;
40  vectorOut[2] = vectorIn[2] * scale2;
41 }
42 
43 static inline void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
44 {
45  vectorOut[0] = vectorIn1[0] + vectorIn2[0];
46  vectorOut[1] = vectorIn1[1] + vectorIn2[1];
47  vectorOut[2] = vectorIn1[2] + vectorIn2[2];
48 }
49 
50 /*
51  #define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \
52  _m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \
53  _m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \
54  _m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \
55  _m_a2c[3] = (_m_b2c[3]*_m_a2b[0] + _m_b2c[4]*_m_a2b[3] + _m_b2c[5]*_m_a2b[6]); \
56  _m_a2c[4] = (_m_b2c[3]*_m_a2b[1] + _m_b2c[4]*_m_a2b[4] + _m_b2c[5]*_m_a2b[7]); \
57  _m_a2c[5] = (_m_b2c[3]*_m_a2b[2] + _m_b2c[4]*_m_a2b[5] + _m_b2c[5]*_m_a2b[8]); \
58  _m_a2c[6] = (_m_b2c[6]*_m_a2b[0] + _m_b2c[7]*_m_a2b[3] + _m_b2c[8]*_m_a2b[6]); \
59  _m_a2c[7] = (_m_b2c[6]*_m_a2b[1] + _m_b2c[7]*_m_a2b[4] + _m_b2c[8]*_m_a2b[7]); \
60  _m_a2c[8] = (_m_b2c[6]*_m_a2b[2] + _m_b2c[7]*_m_a2b[5] + _m_b2c[8]*_m_a2b[8]); \
61  }
62 */
63 
64 static inline void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
65 {
66  float op[3];
67  for (int x = 0; x < 3; x++) {
68  for (int y = 0; y < 3; y++) {
69  for (int w = 0; w < 3; w++) {
70  op[w] = a[x][w] * b[w][y];
71  }
72  mat[x][y] = op[0] + op[1] + op[2];
73  }
74  }
75 }
76 
77 #endif // AHRS_FLOAT_DCM_ALGEBRA_H
static void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
static void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
static float Vector_Dot_Product(float vector1[3], float vector2[3])
static void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)