Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity
Files
ece30810cf76ffee8993f9914551e9bf8c892ee6
paparazzi/sw/airborne/math/pprz_algebra.h
T
Antoine Drouin ece30810cf added VECT3_SUM_SCALED macro
2010-11-04 15:58:05 +00:00

596 lines
18 KiB
C
Raw Blame History

/*
* $Id$
*
* Copyright (C) 2008 Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef PPRZ_ALGEBRA_H
#define PPRZ_ALGEBRA_H
#include <float.h> /* for FLT_EPSILON */
#define SQUARE(_a) ((_a)*(_a))
/*
* Dimension 2 vectors
*/
/* a = {x, y} */
#define VECT2_ASSIGN(_a, _x, _y) { \
(_a).x = (_x); \
(_a).y = (_y); \
}
/* a = b */
#define VECT2_COPY(_a, _b) { \
(_a).x = (_b).x; \
(_a).y = (_b).y; \
}
/* a += b */
#define VECT2_ADD(_a, _b) { \
(_a).x += (_b).x; \
(_a).y += (_b).y; \
}
/* a -= b */
#define VECT2_SUB(_a, _b) { \
(_a).x -= (_b).x; \
(_a).y -= (_b).y; \
}
/* c = a + b */
#define VECT2_SUM(_c, _a, _b) { \
(_c).x = (_a).x + (_b).x; \
(_c).y = (_a).y + (_b).y; \
}
/* c = a - b */
#define VECT2_DIFF(_c, _a, _b) { \
(_c).x = (_a).x - (_b).x; \
(_c).y = (_a).y - (_b).y; \
}
/* _vo = _vi * _s */
#define VECT2_SMUL(_vo, _vi, _s) { \
(_vo).x = (_vi).x * (_s); \
(_vo).y = (_vi).y * (_s); \
}
/* _vo = _vi / _s */
#define VECT2_SDIV(_vo, _vi, _s) { \
(_vo).x = (_vi).x / (_s); \
(_vo).y = (_vi).y / (_s); \
}
/* _v = Bound(_v, _min, _max) */
#define VECT2_STRIM(_v, _min, _max) { \
(_v).x = (_v).x < _min ? _min : (_v).x > _max ? _max : (_v).x; \
(_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
}
/*
* Dimension 3 vectors
*/
/* a = {x, y, z} */
#define VECT3_ASSIGN(_a, _x, _y, _z) { \
(_a).x = (_x); \
(_a).y = (_y); \
(_a).z = (_z); \
}
/* a = b */
#define VECT3_COPY(_a, _b) { \
(_a).x = (_b).x; \
(_a).y = (_b).y; \
(_a).z = (_b).z; \
}
/* a += b */
#define VECT3_ADD(_a, _b) { \
(_a).x += (_b).x; \
(_a).y += (_b).y; \
(_a).z += (_b).z; \
}
/* a -= b */
#define VECT3_SUB(_a, _b) { \
(_a).x -= (_b).x; \
(_a).y -= (_b).y; \
(_a).z -= (_b).z; \
}
/* c = a + b */
#define VECT3_SUM(_c, _a, _b) { \
(_c).x = (_a).x + (_b).x; \
(_c).y = (_a).y + (_b).y; \
(_c).z = (_a).z + (_b).z; \
}
/* c = a + _s * b */
#define VECT3_SUM_SCALED(_c, _a, _b, _s) { \
(_c).x = (_a).x + (_s)*(_b).x; \
(_c).y = (_a).y + (_s)*(_b).y; \
(_c).z = (_a).z + (_s)*(_b).z; \
}
/* c = a - b */
#define VECT3_DIFF(_c, _a, _b) { \
(_c).x = (_a).x - (_b).x; \
(_c).y = (_a).y - (_b).y; \
(_c).z = (_a).z - (_b).z; \
}
/* _vo = _vi * _s */
#define VECT3_SMUL(_vo, _vi, _s) { \
(_vo).x = (_vi).x * (_s); \
(_vo).y = (_vi).y * (_s); \
(_vo).z = (_vi).z * (_s); \
}
/* _vo = _vi / _s */
#define VECT3_SDIV(_vo, _vi, _s) { \
(_vo).x = (_vi).x / (_s); \
(_vo).y = (_vi).y / (_s); \
(_vo).z = (_vi).z / (_s); \
}
/* _v = Bound(_v, _min, _max) */
#define VECT3_STRIM(_v, _min, _max) { \
(_v).x = (_v).x < _min ? _min : (_v).x > _max ? _max : (_v).x; \
(_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
(_v).z = (_v).z < _min ? _min : (_v).z > _max ? _max : (_v).z; \
}
/* */
#define VECT3_EW_DIV(_vo, _va, _vb) { \
(_vo).x = (_va).x / (_vb).x; \
(_vo).y = (_va).y / (_vb).y; \
(_vo).z = (_va).z / (_vb).z; \
}
/* */
#define VECT3_EW_MUL(_vo, _va, _vb) { \
(_vo).x = (_va).x * (_vb).x; \
(_vo).y = (_va).y * (_vb).y; \
(_vo).z = (_va).z * (_vb).z; \
}
/* */
#define VECT3_BOUND_CUBE(_v, _min, _max) { \
if ((_v).x > (_max)) (_v).x = (_max); else if ((_v).x < (_min)) (_v).x = (_min); \
if ((_v).y > (_max)) (_v).y = (_max); else if ((_v).y < (_min)) (_v).y = (_min); \
if ((_v).z > (_max)) (_v).z = (_max); else if ((_v).z < (_min)) (_v).z = (_min); \
}
/* */
#define VECT3_BOUND_BOX(_v, _v_min, _v_max) { \
if ((_v).x > (_v_max.x)) (_v).x = (_v_max.x); else if ((_v).x < (_v_min.x)) (_v).x = (_v_min.x); \
if ((_v).y > (_v_max.y)) (_v).y = (_v_max.y); else if ((_v).y < (_v_min.y)) (_v).y = (_v_min.z); \
if ((_v).z > (_v_max.y)) (_v).z = (_v_max.z); else if ((_v).z < (_v_min.z)) (_v).z = (_v_min.z); \
}
/*
* Euler angles
*/
#define EULERS_COPY(_a, _b) { \
(_a).phi = (_b).phi; \
(_a).theta = (_b).theta; \
(_a).psi = (_b).psi; \
}
#define EULERS_ASSIGN(_e, _phi, _theta, _psi) { \
(_e).phi = _phi; \
(_e).theta = _theta; \
(_e).psi = _psi; \
}
/* a += b */
#define EULERS_ADD(_a, _b) { \
(_a).phi += (_b).phi; \
(_a).theta += (_b).theta; \
(_a).psi += (_b).psi; \
}
/* a += b */
#define EULERS_SUB(_a, _b) { \
(_a).phi -= (_b).phi; \
(_a).theta -= (_b).theta; \
(_a).psi -= (_b).psi; \
}
/* c = a - b */
#define EULERS_DIFF(_c, _a, _b) { \
(_c).phi = (_a).phi - (_b).phi; \
(_c).theta = (_a).theta - (_b).theta; \
(_c).psi = (_a).psi - (_b).psi; \
}
/* _vo = _vi * _s */
#define EULERS_SMUL(_eo, _ei, _s) { \
(_eo).phi = (_ei).phi * (_s); \
(_eo).theta = (_ei).theta * (_s); \
(_eo).psi = (_ei).psi * (_s); \
}
/* _vo = _vi / _s */
#define EULERS_SDIV(_eo, _ei, _s) { \
(_eo).phi = (_ei).phi / (_s); \
(_eo).theta = (_ei).theta / (_s); \
(_eo).psi = (_ei).psi / (_s); \
}
/* _v = Bound(_v, _min, _max) */
#define EULERS_BOUND_CUBE(_v, _min, _max) { \
(_v).phi = (_v).phi < _min ? _min : (_v).phi > _max ? _max : (_v).phi; \
(_v).theta = (_v).theta < _min ? _min : (_v).theta > _max ? _max : (_v).theta; \
(_v).psi = (_v).psi < _min ? _min : (_v).psi > _max ? _max : (_v).psi; \
}
/*
* Rates
*/
/* ra = {p, q, r} */
#define RATES_ASSIGN(_ra, _p, _q, _r) { \
(_ra).p = (_p); \
(_ra).q = (_q); \
(_ra).r = (_r); \
}
/* a = b */
#define RATES_COPY(_a, _b) { \
(_a).p = (_b).p; \
(_a).q = (_b).q; \
(_a).r = (_b).r; \
}
/* a += b */
#define RATES_ADD(_a, _b) { \
(_a).p += (_b).p; \
(_a).q += (_b).q; \
(_a).r += (_b).r; \
}
/* a -= b */
#define RATES_SUB(_a, _b) { \
(_a).p -= (_b).p; \
(_a).q -= (_b).q; \
(_a).r -= (_b).r; \
}
/* c = a + b */
#define RATES_SUM(_c, _a, _b) { \
(_c).p = (_a).p + (_b).p; \
(_c).q = (_a).q + (_b).q; \
(_c).r = (_a).r + (_b).r; \
}
/* c = a - b */
#define RATES_DIFF(_c, _a, _b) { \
(_c).p = (_a).p - (_b).p; \
(_c).q = (_a).q - (_b).q; \
(_c).r = (_a).r - (_b).r; \
}
/* _ro = _ri * _s */
#define RATES_SMUL(_ro, _ri, _s) { \
(_ro).p = (_ri).p * (_s); \
(_ro).q = (_ri).q * (_s); \
(_ro).r = (_ri).r * (_s); \
}
/* _ro = _ri / _s */
#define RATES_SDIV(_ro, _ri, _s) { \
(_ro).p = (_ri).p / (_s) ; \
(_ro).q = (_ri).q / (_s); \
(_ro).r = (_ri).r / (_s); \
}
/* Element wise vector multiplication */
#define RATES_EWMULT_RSHIFT(c, a, b, _s) { \
c.p = (a.p * b.p) >> (_s); \
c.q = (a.q * b.q) >> (_s); \
c.r = (a.r * b.r) >> (_s); \
}
/* _v = Bound(_v, _min, _max) */
#define RATES_BOUND_CUBE(_v, _min, _max) { \
(_v).p = (_v).p < _min ? _min : (_v).p > _max ? _max : (_v).p; \
(_v).q = (_v).q < _min ? _min : (_v).q > _max ? _max : (_v).q; \
(_v).r = (_v).r < _min ? _min : (_v).r > _max ? _max : (_v).r; \
}
#define RATES_BOUND_BOX(_v, _v_min, _v_max) { \
if ((_v).p > (_v_max.p)) (_v).p = (_v_max.p); else if ((_v).p < (_v_min.p)) (_v).p = (_v_min.p); \
if ((_v).q > (_v_max.q)) (_v).q = (_v_max.q); else if ((_v).q < (_v_min.q)) (_v).q = (_v_min.q); \
if ((_v).r > (_v_max.r)) (_v).r = (_v_max.r); else if ((_v).r < (_v_min.r)) (_v).r = (_v_min.r); \
}
/*
* 3x3 matrices
*/
/* accessor : row and col range from 0 to 2 */
#define MAT33_ELMT(_m, _row, _col) ((_m).m[_row*3+_col])
#define MAT33_COPY(_mat1,_mat2) { \
MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat2),0,0); \
MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat2),0,1); \
MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat2),0,2); \
MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat2),1,0); \
MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat2),1,1); \
MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat2),1,2); \
MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat2),2,0); \
MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat2),2,1); \
MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat2),2,2); \
}
/* multiply _vin by _mat, store in _vout */
#define MAT33_VECT3_MUL(_vout, _mat, _vin) { \
(_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
MAT33_ELMT((_mat), 0, 1) * (_vin).y + \
MAT33_ELMT((_mat), 0, 2) * (_vin).z; \
(_vout).y = MAT33_ELMT((_mat), 1, 0) * (_vin).x + \
MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
MAT33_ELMT((_mat), 1, 2) * (_vin).z; \
(_vout).z = MAT33_ELMT((_mat), 2, 0) * (_vin).x + \
MAT33_ELMT((_mat), 2, 1) * (_vin).y + \
MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
}
/* multiply _vin by transpose of _mat, store in _vout */
#define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin) { \
(_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
MAT33_ELMT((_mat), 1, 0) * (_vin).y + \
MAT33_ELMT((_mat), 2, 0) * (_vin).z; \
(_vout).y = MAT33_ELMT((_mat), 0, 1) * (_vin).x + \
MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
MAT33_ELMT((_mat), 2, 1) * (_vin).z; \
(_vout).z = MAT33_ELMT((_mat), 0, 2) * (_vin).x + \
MAT33_ELMT((_mat), 1, 2) * (_vin).y + \
MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
}
/* invS = 1/det(S) com(S)' */
#define MAT33_INV(_minv, _m) { \
const float m00 = MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,1); \
const float m10 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,1); \
const float m20 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,1); \
const float m01 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,0); \
const float m11 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,0); \
const float m21 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,0); \
const float m02 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,0); \
const float m12 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,0); \
const float m22 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,0); \
const float det = MAT33_ELMT((_m),0,0)*m00 - MAT33_ELMT((_m),1,0)*m10 + MAT33_ELMT((_m),2,0)*m20; \
if (fabs(det) > FLT_EPSILON) { \
MAT33_ELMT((_minv),0,0) = m00 / det; \
MAT33_ELMT((_minv),1,0) = -m01 / det; \
MAT33_ELMT((_minv),2,0) = m02 / det; \
MAT33_ELMT((_minv),0,1) = -m10 / det; \
MAT33_ELMT((_minv),1,1) = m11 / det; \
MAT33_ELMT((_minv),2,1) = -m12 / det; \
MAT33_ELMT((_minv),0,2) = m20 / det; \
MAT33_ELMT((_minv),1,2) = -m21 / det; \
MAT33_ELMT((_minv),2,2) = m22 / det; \
} \
}
//
//
// Quaternions
//
//
#define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
(_q).qi = (_i); \
(_q).qx = (_x); \
(_q).qy = (_y); \
(_q).qz = (_z); \
}
#define QUAT_DIFF(_qc, _qa, _qb) { \
(_qc).qi = (_qa).qi - (_qb).qi; \
(_qc).qx = (_qa).qx - (_qb).qx; \
(_qc).qy = (_qa).qy - (_qb).qy; \
(_qc).qz = (_qa).qz - (_qb).qz; \
}
#define QUAT_COPY(_qo, _qi) { \
(_qo).qi = (_qi).qi; \
(_qo).qx = (_qi).qx; \
(_qo).qy = (_qi).qy; \
(_qo).qz = (_qi).qz; \
}
#define QUAT_EXPLEMENTARY(b,a) { \
b.qi = -a.qi; \
b.qx = -a.qx; \
b.qy = -a.qy; \
b.qz = -a.qz; \
}
#define QUAT_SMUL(_qo, _qi, _s) { \
(_qo).qi = (_qi).qi * _s; \
(_qo).qx = (_qi).qx * _s; \
(_qo).qy = (_qi).qy * _s; \
(_qo).qz = (_qi).qz * _s; \
}
#define QUAT_ADD(_qo, _qi) { \
(_qo).qi += (_qi).qi; \
(_qo).qx += (_qi).qx; \
(_qo).qy += (_qi).qy; \
(_qo).qz += (_qi).qz; \
}
#define QUAT_INVERT(_qo, _qi) { \
(_qo).qi = (_qi).qi; \
(_qo).qx = -(_qi).qx; \
(_qo).qy = -(_qi).qy; \
(_qo).qz = -(_qi).qz; \
}
/*
* Rotation Matrices
*/
/* accessor : row and col range from 0 to 2 */
#define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
/* trace */
#define RMAT_TRACE(_rm) (RMAT_ELMT(_rm, 0, 0)+RMAT_ELMT(_rm, 1, 1)+RMAT_ELMT(_rm, 2, 2))
#define RMAT_DIFF(_c, _a, _b) { \
(_c).m[0] = (_a).m[0] - (_b).m[0]; \
(_c).m[1] = (_a).m[1] - (_b).m[1]; \
(_c).m[2] = (_a).m[2] - (_b).m[2]; \
(_c).m[3] = (_a).m[3] - (_b).m[3]; \
(_c).m[4] = (_a).m[4] - (_b).m[4]; \
(_c).m[5] = (_a).m[5] - (_b).m[5]; \
(_c).m[6] = (_a).m[6] - (_b).m[6]; \
(_c).m[7] = (_a).m[7] - (_b).m[7]; \
(_c).m[8] = (_a).m[8] - (_b).m[8]; \
}
/* multiply _vin by _rmat, store in _vout */
#define RMAT_VECT3_MUL(_vout, _rmat, _vin) { \
(_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
RMAT_ELMT((_rmat), 0, 1) * (_vin).y + \
RMAT_ELMT((_rmat), 0, 2) * (_vin).z; \
(_vout).y = RMAT_ELMT((_rmat), 1, 0) * (_vin).x + \
RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
RMAT_ELMT((_rmat), 1, 2) * (_vin).z; \
(_vout).z = RMAT_ELMT((_rmat), 2, 0) * (_vin).x + \
RMAT_ELMT((_rmat), 2, 1) * (_vin).y + \
RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
}
#define RMAT_COPY(_o, _i) { memcpy(&(_o), &(_i), sizeof(_o));}
#define EULERS_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).phi = ANGLE_FLOAT_OF_BFP((_ei).phi); \
(_ef).theta = ANGLE_FLOAT_OF_BFP((_ei).theta); \
(_ef).psi = ANGLE_FLOAT_OF_BFP((_ei).psi); \
}
#define EULERS_BFP_OF_REAL(_ei, _ef) { \
(_ei).phi = ANGLE_BFP_OF_REAL((_ef).phi); \
(_ei).theta = ANGLE_BFP_OF_REAL((_ef).theta); \
(_ei).psi = ANGLE_BFP_OF_REAL((_ef).psi); \
}
#define RMAT_BFP_OF_REAL(_ei, _ef) { \
(_ei).m[0] = TRIG_BFP_OF_REAL((_ef).m[0]); \
(_ei).m[1] = TRIG_BFP_OF_REAL((_ef).m[1]); \
(_ei).m[2] = TRIG_BFP_OF_REAL((_ef).m[2]); \
(_ei).m[3] = TRIG_BFP_OF_REAL((_ef).m[3]); \
(_ei).m[4] = TRIG_BFP_OF_REAL((_ef).m[4]); \
(_ei).m[5] = TRIG_BFP_OF_REAL((_ef).m[5]); \
(_ei).m[6] = TRIG_BFP_OF_REAL((_ef).m[6]); \
(_ei).m[7] = TRIG_BFP_OF_REAL((_ef).m[7]); \
(_ei).m[8] = TRIG_BFP_OF_REAL((_ef).m[8]); \
}
#define RMAT_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).m[0] = TRIG_FLOAT_OF_BFP((_ei).m[0]); \
(_ef).m[1] = TRIG_FLOAT_OF_BFP((_ei).m[1]); \
(_ef).m[2] = TRIG_FLOAT_OF_BFP((_ei).m[2]); \
(_ef).m[3] = TRIG_FLOAT_OF_BFP((_ei).m[3]); \
(_ef).m[4] = TRIG_FLOAT_OF_BFP((_ei).m[4]); \
(_ef).m[5] = TRIG_FLOAT_OF_BFP((_ei).m[5]); \
(_ef).m[6] = TRIG_FLOAT_OF_BFP((_ei).m[6]); \
(_ef).m[7] = TRIG_FLOAT_OF_BFP((_ei).m[7]); \
(_ef).m[8] = TRIG_FLOAT_OF_BFP((_ei).m[8]); \
}
#define QUAT_FLOAT_OF_BFP(_qf, _qi) { \
(_qf).qi = QUAT1_FLOAT_OF_BFP((_qi).qi); \
(_qf).qx = QUAT1_FLOAT_OF_BFP((_qi).qx); \
(_qf).qy = QUAT1_FLOAT_OF_BFP((_qi).qy); \
(_qf).qz = QUAT1_FLOAT_OF_BFP((_qi).qz); \
}
#define QUAT_BFP_OF_REAL(_qi, _qf) { \
(_qi).qi = QUAT1_BFP_OF_REAL((_qf).qi); \
(_qi).qx = QUAT1_BFP_OF_REAL((_qf).qx); \
(_qi).qy = QUAT1_BFP_OF_REAL((_qf).qy); \
(_qi).qz = QUAT1_BFP_OF_REAL((_qf).qz); \
}
#define RATES_FLOAT_OF_BFP(_rf, _ri) { \
(_rf).p = RATE_FLOAT_OF_BFP((_ri).p); \
(_rf).q = RATE_FLOAT_OF_BFP((_ri).q); \
(_rf).r = RATE_FLOAT_OF_BFP((_ri).r); \
}
#define RATES_BFP_OF_REAL(_ri, _rf) { \
(_ri).p = RATE_BFP_OF_REAL((_rf).p); \
(_ri).q = RATE_BFP_OF_REAL((_rf).q); \
(_ri).r = RATE_BFP_OF_REAL((_rf).r); \
}
#define ACCELS_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
(_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
(_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
}
#define ACCELS_BFP_OF_REAL(_ef, _ei) { \
(_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
(_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
(_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
}
#define MAGS_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).x = MAG_FLOAT_OF_BFP((_ei).x); \
(_ef).y = MAG_FLOAT_OF_BFP((_ei).y); \
(_ef).z = MAG_FLOAT_OF_BFP((_ei).z); \
}
#define MAGS_BFP_OF_REAL(_ef, _ei) { \
(_ef).x = MAG_BFP_OF_REAL((_ei).x); \
(_ef).y = MAG_BFP_OF_REAL((_ei).y); \
(_ef).z = MAG_BFP_OF_REAL((_ei).z); \
}
#endif /* PPRZ_ALGEBRA_H */
Reference in New Issue View Git Blame Copy Permalink