Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ahrs_float_dcm_algebra.h
Go to the documentation of this file.
1 //Algebra helper functions for DCM
2 
3 static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
4 {
5  return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2];
6 }
7 
8 static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
9 {
10  vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
11  vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
12  vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
13 }
14 
15 static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
16 {
17  vectorOut[0]=vectorIn[0]*scale2;
18  vectorOut[1]=vectorIn[1]*scale2;
19  vectorOut[2]=vectorIn[2]*scale2;
20 }
21 
22 static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
23 {
24  vectorOut[0]=vectorIn1[0]+vectorIn2[0];
25  vectorOut[1]=vectorIn1[1]+vectorIn2[1];
26  vectorOut[2]=vectorIn1[2]+vectorIn2[2];
27 }
28 
29 /*
30  #define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \
31  _m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \
32  _m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \
33  _m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \
34  _m_a2c[3] = (_m_b2c[3]*_m_a2b[0] + _m_b2c[4]*_m_a2b[3] + _m_b2c[5]*_m_a2b[6]); \
35  _m_a2c[4] = (_m_b2c[3]*_m_a2b[1] + _m_b2c[4]*_m_a2b[4] + _m_b2c[5]*_m_a2b[7]); \
36  _m_a2c[5] = (_m_b2c[3]*_m_a2b[2] + _m_b2c[4]*_m_a2b[5] + _m_b2c[5]*_m_a2b[8]); \
37  _m_a2c[6] = (_m_b2c[6]*_m_a2b[0] + _m_b2c[7]*_m_a2b[3] + _m_b2c[8]*_m_a2b[6]); \
38  _m_a2c[7] = (_m_b2c[6]*_m_a2b[1] + _m_b2c[7]*_m_a2b[4] + _m_b2c[8]*_m_a2b[7]); \
39  _m_a2c[8] = (_m_b2c[6]*_m_a2b[2] + _m_b2c[7]*_m_a2b[5] + _m_b2c[8]*_m_a2b[8]); \
40  }
41 */
42 
43 static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
44 {
45  float op[3];
46  for(int x=0; x<3; x++)
47  {
48  for(int y=0; y<3; y++)
49  {
50  for(int w=0; w<3; w++)
51  {
52  op[w]=a[x][w]*b[w][y];
53  }
54  mat[x][y]=op[0]+op[1]+op[2];
55  }
56  }
57 }
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)