Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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  {
69  for(int y=0; y<3; y++)
70  {
71  for(int w=0; w<3; w++)
72  {
73  op[w]=a[x][w]*b[w][y];
74  }
75  mat[x][y]=op[0]+op[1]+op[2];
76  }
77  }
78 }
79 
80 #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)