Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
24static 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
29static 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
36static 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
43static 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
64static 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 Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
static void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
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])
uint16_t foo
Definition main_demo5.c:58
float b
Definition wedgebug.c:202