Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
Macros
pprz_simple_matrix.h File Reference

Simple matrix helper macros. More...

#include <float.h>
#include <math.h>
+ Include dependency graph for pprz_simple_matrix.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Macros

#define warn_message(...)   do { } while(0)
 
#define MAT_MUL(_i, _k, _j, C, A, B)
 
#define MAT_MUL_T(_i, _k, _j, C, A, B)
 
#define MAT_SUB(_i, _j, C, A, B)
 
#define MAT_MUL_VECT(_n, o, a, b)
 o = a * b More...
 
#define MAT_INV33(_invS, _S)
 
#define MAT_SUM(_i, _j, C, A, B)
 
#define MAT_SUM_c(_i, _j, C, A, B, k_)
 
#define MAT_MUL_c(_i, _k, _j, C, A, B, c_)
 
#define MAT_PRINT(_i, _j, A)
 

Detailed Description

Simple matrix helper macros.

Definition in file pprz_simple_matrix.h.

Macro Definition Documentation

#define MAT_INV33 (   _invS,
  _S 
)
Value:
{ \
const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1]; \
const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1]; \
const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1]; \
const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0]; \
const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0]; \
const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0]; \
const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0]; \
const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0]; \
const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0]; \
float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20; \
if (fabs(det) < FLT_EPSILON) { \
/* If the determinant is too small then set it to epsilon preserving sign. */ \
warn_message("warning: %s:%d MAT_INV33 trying to invert non-invertable matrix '%s' and put result in '%s'.\n", __FILE__, __LINE__, #_S, #_invS); \
det = copysignf(FLT_EPSILON, det); \
} \
_invS[0][0] = m00 / det; \
_invS[1][0] = -m01 / det; \
_invS[2][0] = m02 / det; \
_invS[0][1] = -m10 / det; \
_invS[1][1] = m11 / det; \
_invS[2][1] = -m12 / det; \
_invS[0][2] = m20 / det; \
_invS[1][2] = -m21 / det; \
_invS[2][2] = m22 / det; \
}
warn_message
#define warn_message(...)
Definition: pprz_simple_matrix.h:40

Definition at line 99 of file pprz_simple_matrix.h.

Referenced by update_state(), and update_state_heading().

#define MAT_MUL (   _i,
  _k,
  _j,
  C,
  A,
  B 
)
Value:
{ \
int l,c,m; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) { \
C[l][c] = 0.; \
for (m=0; m<_k; m++) \
C[l][c] += A[l][m]*B[m][c]; \
} \
}
B
#define B
Definition: ahrs_float_invariant.c:101
A
#define A
Definition: pprz_geodetic_utm.h:44

Definition at line 46 of file pprz_simple_matrix.h.

Referenced by fit_linear_flow_field(), fit_linear_model(), fit_linear_model_prior(), propagate_state(), update_state(), and update_state_heading().

#define MAT_MUL_c (   _i,
  _k,
  _j,
  C,
  A,
  B,
  c_ 
)
Value:
{ \
int l,c,m; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) { \
C[l][c] = 0.; \
for (m=0; m<_k; m++) \
C[l][c] += c_*A[l][m]*B[m][c]; \
} \
}
B
#define B
Definition: ahrs_float_invariant.c:101
A
#define A
Definition: pprz_geodetic_utm.h:44

Definition at line 150 of file pprz_simple_matrix.h.

#define MAT_MUL_T (   _i,
  _k,
  _j,
  C,
  A,
  B 
)
Value:
{ \
int l,c,m; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) { \
C[l][c] = 0.; \
for (m=0; m<_k; m++) \
C[l][c] += A[l][m]*B[c][m]; \
} \
}
B
#define B
Definition: ahrs_float_invariant.c:101
A
#define A
Definition: pprz_geodetic_utm.h:44

Definition at line 59 of file pprz_simple_matrix.h.

Referenced by propagate_state(), update_state(), and update_state_heading().

#define MAT_MUL_VECT (   _n,
  o,
  a,
  b 
)
Value:
{ \
int l,m; \
for (l = 0; l < _n; l++) { \
o[l] = 0; \
for (m = 0; m < _n; m++) { \
o[l] += a[l][m] * b[m]; \
} \
} \
}

o = a * b

a: [n x n] b: [n x 1] o: [n x 1]

Definition at line 86 of file pprz_simple_matrix.h.

Referenced by stabilization_attitude_run().

#define MAT_PRINT (   _i,
  _j,
  A 
)
Value:
{ \
int l,c; \
printf("float "); \
printf(#A); \
printf(" = {\n"); \
for (l=0; l<_i; l++){ \
printf("{"); \
for (c=0; c<_j; c++){ \
if(c<(_j-1)){printf("%f,",A[l][c]);} \
else{printf("%f",A[l][c]);} \
} \
if(l<(_i-1)){ \
printf("} ,\n");} \
else{printf("}\n");} \
} \
printf("}\n"); \
}
A
#define A
Definition: pprz_geodetic_utm.h:44

Definition at line 161 of file pprz_simple_matrix.h.

#define MAT_SUB (   _i,
  _j,
  C,
  A,
  B 
)
Value:
{ \
int l,c; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) \
C[l][c] = A[l][c] - B[l][c]; \
}
B
#define B
Definition: ahrs_float_invariant.c:101
A
#define A
Definition: pprz_geodetic_utm.h:44

Definition at line 73 of file pprz_simple_matrix.h.

Referenced by fit_linear_flow_field(), fit_linear_model(), fit_linear_model_prior(), update_state(), and update_state_heading().

#define MAT_SUM (   _i,
  _j,
  C,
  A,
  B 
)
Value:
{ \
int l,c; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) \
C[l][c] = A[l][c] + B[l][c]; \
}
B
#define B
Definition: ahrs_float_invariant.c:101
A
#define A
Definition: pprz_geodetic_utm.h:44

Definition at line 129 of file pprz_simple_matrix.h.

#define MAT_SUM_c (   _i,
  _j,
  C,
  A,
  B,
  k_ 
)
Value:
{ \
int l,c; \
for (l=0; l<_i; l++) \
for (c=0; c<_j; c++) \
C[l][c] = A[l][c] + B[l][c]*k_; \
}
B
#define B
Definition: ahrs_float_invariant.c:101
A
#define A
Definition: pprz_geodetic_utm.h:44

Definition at line 140 of file pprz_simple_matrix.h.

#define warn_message (   ...)    do { } while(0)

Definition at line 40 of file pprz_simple_matrix.h.