CFLOAT
M_DECL_FUNC (__catanh) (CFLOAT x)
{
CFLOAT res;
int rcls = fpclassify (__real__ x);
int icls = fpclassify (__imag__ x);
if (__glibc_unlikely (rcls <= FP_INFINITE || icls <= FP_INFINITE))
{
if (icls == FP_INFINITE)
{
__real__ res = M_COPYSIGN (0, __real__ x);
__imag__ res = M_COPYSIGN (M_MLIT (M_PI_2), __imag__ x);
}
else if (rcls == FP_INFINITE || rcls == FP_ZERO)
{
__real__ res = M_COPYSIGN (0, __real__ x);
if (icls >= FP_ZERO)
__imag__ res = M_COPYSIGN (M_MLIT (M_PI_2), __imag__ x);
else
__imag__ res = M_NAN;
}
else
{
__real__ res = M_NAN;
__imag__ res = M_NAN;
}
}
else if (__glibc_unlikely (rcls == FP_ZERO && icls == FP_ZERO))
{
res = x;
}
else
{
if (M_FABS (__real__ x) >= 16 / M_EPSILON
|| M_FABS (__imag__ x) >= 16 / M_EPSILON)
{
__imag__ res = M_COPYSIGN (M_MLIT (M_PI_2), __imag__ x);
if (M_FABS (__imag__ x) <= 1)
__real__ res = 1 / __real__ x;
else if (M_FABS (__real__ x) <= 1)
__real__ res = __real__ x / __imag__ x / __imag__ x;
else
{
FLOAT h = M_HYPOT (__real__ x / 2, __imag__ x / 2);
__real__ res = __real__ x / h / h / 4;
}
}
else
{
if (M_FABS (__real__ x) == 1
&& M_FABS (__imag__ x) < M_EPSILON * M_EPSILON)
__real__ res = (M_COPYSIGN (M_LIT (0.5), __real__ x)
* ((FLOAT) M_MLIT (M_LN2)
- M_LOG (M_FABS (__imag__ x))));
else
{
FLOAT i2 = 0;
if (M_FABS (__imag__ x) >= M_EPSILON * M_EPSILON)
i2 = __imag__ x * __imag__ x;
FLOAT num = 1 + __real__ x;
num = i2 + num * num;
FLOAT den = 1 - __real__ x;
den = i2 + den * den;
FLOAT f = num / den;
if (f < M_LIT (0.5))
__real__ res = M_LIT (0.25) * M_LOG (f);
else
{
num = 4 * __real__ x;
__real__ res = M_LIT (0.25) * M_LOG1P (num / den);
}
}
FLOAT absx, absy, den;
absx = M_FABS (__real__ x);
absy = M_FABS (__imag__ x);
if (absx < absy)
{
FLOAT t = absx;
absx = absy;
absy = t;
}
if (absy < M_EPSILON / 2)
{
den = (1 - absx) * (1 + absx);
if (den == 0)
den = 0;
}
else if (absx >= 1)
den = (1 - absx) * (1 + absx) - absy * absy;
else if (absx >= M_LIT (0.75) || absy >= M_LIT (0.5))
den = -M_SUF (__x2y2m1) (absx, absy);
else
den = (1 - absx) * (1 + absx) - absy * absy;
__imag__ res = M_LIT (0.5) * M_ATAN2 (2 * __imag__ x, den);
}
math_check_force_underflow_complex (res);
}
return res;
}
/**
* full state strap down IMU emulation function.
* output IS A/V state as sensed at IMU given location.
*
* @param {struct} IMU inputs
* @return {struct} IMU outputs
**/
void F_IMU(struct imu_inputs_def *pIn, struct imu_outputs_def *pOut) {
// locals
double _omega[3][3], // rotational velocity matrix
_earth2body[3][3], // earth -> body transformation matrix
_body2earth[3][3], // body -> earth transformation matrix
_imu2body[3][3], // IMU -> body transformation matrix
_body2imu[3][3], // body -> IMU transformation matrix
_geodeticCG[3], // {lat, long, alt} of the cg [rad, rad, ft]
_matTemp1[3][3], // temporary matrix
_vectorTemp1[3], // temporary vector
_VectorTemp2[3], // temporary vector
_vectorTemp3[3], // temporary vector
_lat, _lon; // latitude and longitude [rad]
// construct Euler "omega cross" matrix
_omega[_X][_X] = 0.0;
_omega[_X][_Y] = -pIn->cg_pqr[_Z];
_omega[_X][_Z] = pIn->cg_pqr[_Y];
_omega[_Y][_X] = pIn->cg_pqr[_Z];
_omega[_Y][_Y] = 0.0;
_omega[_Y][_Z] = -pIn->cg_pqr[_X];
_omega[_Z][_X] = -pIn->cg_pqr[_Y];
_omega[_Z][_Y] = pIn->cg_pqr[_X];
_omega[_Z][_Z] = 0.0;
// create earth to body transformation matrix
F_EULER_TO_DCM(_earth2body, pIn->cg_THETA[_PHI], pIn->cg_THETA[_THETA], pIn->cg_THETA[_PSI]);
// create body to earth transformation matrix
M_MAT_TRANSPOSE(_body2earth, _earth2body);
// create body to IMU transformation matrix
memcpy(_body2imu, pIn->body2imu, 9 * sizeof(double));
// create IMU to body transformation matrix
M_MAT_TRANSPOSE(_imu2body, _body2imu);
// calculate C.G location in geodetic coordinate system [ft]
_vectorTemp1[_X] = pIn->cg_pos[_X] * C_FOOT_TO_METER;
_vectorTemp1[_Y] = pIn->cg_pos[_Y] * C_FOOT_TO_METER;
_vectorTemp1[_Z] = pIn->cg_pos[_Z] * C_FOOT_TO_METER;
F_ECEF_TO_GEODETIC(_vectorTemp1, _geodeticCG);
_geodeticCG[_Z] = _geodeticCG[_Z] * C_METER_TO_FOOT;
_lat = _geodeticCG[_X];
_lon = _geodeticCG[_Y];
// calculate acceleration vector at IMU location ( a_imu = _body2imu * (a_cg + (alpha X position) + (w X (w X position)) )
// alpha X position
_vectorTemp1[_X] = pIn->cg_alpha[_Y] * pIn->cg2imu[_Z] - pIn->cg_alpha[_Z] * pIn->cg2imu[_Y];
_vectorTemp1[_Y] = pIn->cg_alpha[_Z] * pIn->cg2imu[_X] - pIn->cg_alpha[_X] * pIn->cg2imu[_Z];
_vectorTemp1[_Z] = pIn->cg_alpha[_X] * pIn->cg2imu[_Y] - pIn->cg_alpha[_Y] * pIn->cg2imu[_X];
// _omega X _omega
_matTemp1[_X][_X] = _omega[_X][_X] * _omega[_X][_X] + _omega[_X][_Y] * _omega[_Y][_X] + _omega[_X][_Z] * _omega[_Z][_X];
_matTemp1[_X][_Y] = _omega[_X][_X] * _omega[_X][_Y] + _omega[_X][_Y] * _omega[_Y][_Y] + _omega[_X][_Z] * _omega[_Z][_Y];
_matTemp1[_X][_Z] = _omega[_X][_X] * _omega[_X][_Z] + _omega[_X][_Y] * _omega[_Y][_Z] + _omega[_X][_Z] * _omega[_Z][_Z];
_matTemp1[_Y][_X] = _omega[_Y][_X] * _omega[_X][_X] + _omega[_Y][_Y] * _omega[_Y][_X] + _omega[_Y][_Z] * _omega[_Z][_X];
_matTemp1[_Y][_Y] = _omega[_Y][_X] * _omega[_X][_Y] + _omega[_Y][_Y] * _omega[_Y][_Y] + _omega[_Y][_Z] * _omega[_Z][_Y];
_matTemp1[_Y][_Z] = _omega[_Y][_X] * _omega[_X][_Z] + _omega[_Y][_Y] * _omega[_Y][_Z] + _omega[_Y][_Z] * _omega[_Z][_Z];
_matTemp1[_Z][_X] = _omega[_Z][_X] * _omega[_X][_X] + _omega[_Z][_Y] * _omega[_Y][_X] + _omega[_Z][_Z] * _omega[_Z][_X];
_matTemp1[_Z][_Y] = _omega[_Z][_X] * _omega[_X][_Y] + _omega[_Z][_Y] * _omega[_Y][_Y] + _omega[_Z][_Z] * _omega[_Z][_Y];
_matTemp1[_Z][_Z] = _omega[_Z][_X] * _omega[_X][_Z] + _omega[_Z][_Y] * _omega[_Y][_Z] + _omega[_Z][_Z] * _omega[_Z][_Z];
// (_omega X _omega) X position
_VectorTemp2[_X] = _matTemp1[_X][_X] * pIn->cg2imu[_X] + _matTemp1[_X][_Y] * pIn->cg2imu[_Y] + _matTemp1[_X][_Z] * pIn->cg2imu[_Z];
_VectorTemp2[_Y] = _matTemp1[_Y][_X] * pIn->cg2imu[_X] + _matTemp1[_Y][_Y] * pIn->cg2imu[_Y] + _matTemp1[_Y][_Z] * pIn->cg2imu[_Z];
_VectorTemp2[_Z] = _matTemp1[_Z][_X] * pIn->cg2imu[_X] + _matTemp1[_Z][_Y] * pIn->cg2imu[_Y] + _matTemp1[_Z][_Z] * pIn->cg2imu[_Z];
// (_omega X _omega) X position + (alpha X position)
_vectorTemp3[_X] = _vectorTemp1[_X] + _VectorTemp2[_X];
_vectorTemp3[_Y] = _vectorTemp1[_Y] + _VectorTemp2[_Y];
_vectorTemp3[_Z] = _vectorTemp1[_Z] + _VectorTemp2[_Z];
// a_cg + (_omega X _omega) X position + (alpha X position)
_vectorTemp1[_X] = _vectorTemp3[_X] + pIn->cg_accel[_X];
_vectorTemp1[_Y] = _vectorTemp3[_Y] + pIn->cg_accel[_Y];
_vectorTemp1[_Z] = _vectorTemp3[_Z] + pIn->cg_accel[_Z];
// a_imu = _body2imu * a_cg
pOut->accel[_X] = _body2imu[_X][_X] * _vectorTemp1[_X] + _body2imu[_X][_Y] * _vectorTemp1[_Y] + _body2imu[_X][_Z] * _vectorTemp1[_Z];
pOut->accel[_Y] = _body2imu[_Y][_X] * _vectorTemp1[_X] + _body2imu[_Y][_Y] * _vectorTemp1[_Y] + _body2imu[_Y][_Z] * _vectorTemp1[_Z];
pOut->accel[_Z] = _body2imu[_Z][_X] * _vectorTemp1[_X] + _body2imu[_Z][_Y] * _vectorTemp1[_Y] + _body2imu[_Z][_Z] * _vectorTemp1[_Z];
// calculate velocity vector at IMU location (v_imu = _body2imu * (v_cg + (w X pos)) )
// w X position
_vectorTemp1[_X] = _omega[_X][_X] * pIn->cg2imu[_X] + _omega[_X][_Y] * pIn->cg2imu[_Y] + _omega[_X][_Z] * pIn->cg2imu[_Z];
_vectorTemp1[_Y] = _omega[_Y][_X] * pIn->cg2imu[_X] + _omega[_Y][_Y] * pIn->cg2imu[_Y] + _omega[_Y][_Z] * pIn->cg2imu[_Z];
_vectorTemp1[_Z] = _omega[_Z][_X] * pIn->cg2imu[_X] + _omega[_Z][_Y] * pIn->cg2imu[_Y] + _omega[_Z][_Z] * pIn->cg2imu[_Z];
// v_cg + (w X position)
_VectorTemp2[_X] = _vectorTemp1[_X] + pIn->cg_uvw[_X];
_VectorTemp2[_Y] = _vectorTemp1[_Y] + pIn->cg_uvw[_Y];
_VectorTemp2[_Z] = _vectorTemp1[_Z] + pIn->cg_uvw[_Z];
// v_imu = _body2imu * (v_cg + (w X position))
pOut->uvw[_X] = _body2imu[_X][_X] * _VectorTemp2[_X] + _body2imu[_X][_Y] * _VectorTemp2[_Y] + _body2imu[_X][_Z] * _VectorTemp2[_Z];
pOut->uvw[_Y] = _body2imu[_Y][_X] * _VectorTemp2[_X] + _body2imu[_Y][_Y] * _VectorTemp2[_Y] + _body2imu[_Y][_Z] * _VectorTemp2[_Z];
pOut->uvw[_Z] = _body2imu[_Z][_X] * _VectorTemp2[_X] + _body2imu[_Z][_Y] * _VectorTemp2[_Y] + _body2imu[_Z][_Z] * _VectorTemp2[_Z];
// rotate IMU position in body coordinate system to TP coordinates
_vectorTemp1[_X] = _body2earth[_X][_X] * pIn->cg2imu[_X] + _body2earth[_X][_Y] * pIn->cg2imu[_Y] + _body2earth[_X][_Z] * pIn->cg2imu[_Z];
_vectorTemp1[_Y] = _body2earth[_Y][_X] * pIn->cg2imu[_X] + _body2earth[_Y][_Y] * pIn->cg2imu[_Y] + _body2earth[_Y][_Z] * pIn->cg2imu[_Z];
_vectorTemp1[_Z] = _body2earth[_Z][_X] * pIn->cg2imu[_X] + _body2earth[_Z][_Y] * pIn->cg2imu[_Y] + _body2earth[_Z][_Z] * pIn->cg2imu[_Z];
// transform position to ECEF coordinate system
F_GEODETIC_TO_ECEF(_VectorTemp2, _vectorTemp1, _lat, _lon);
// calculate IMU position in ECEF coordinate system
pOut->ECEFpos[_X] = _VectorTemp2[_X] + pIn->cg_pos[_X];
pOut->ECEFpos[_Y] = _VectorTemp2[_Y] + pIn->cg_pos[_Y];
pOut->ECEFpos[_Z] = _VectorTemp2[_Z] + pIn->cg_pos[_Z];
// calculate IMU position in geodetic coordinate system [ft]
_vectorTemp1[_X] = pOut->ECEFpos[_X] * C_FOOT_TO_METER;
_vectorTemp1[_Y] = pOut->ECEFpos[_Y] * C_FOOT_TO_METER;
_vectorTemp1[_Z] = pOut->ECEFpos[_Z] * C_FOOT_TO_METER;
F_ECEF_TO_GEODETIC(_vectorTemp1, pOut->LLHpos);
pOut->LLHpos[_ALT] = pOut->LLHpos[_ALT] * C_METER_TO_FOOT;
// calculate IMU angular velocity vector
pOut->pqr[_X] = _body2imu[_X][_X] * pIn->cg_pqr[_X] + _body2imu[_X][_Y] * pIn->cg_pqr[_Y] + _body2imu[_X][_Z] * pIn->cg_pqr[_Z];
pOut->pqr[_Y] = _body2imu[_Y][_X] * pIn->cg_pqr[_X] + _body2imu[_Y][_Y] * pIn->cg_pqr[_Y] + _body2imu[_Y][_Z] * pIn->cg_pqr[_Z];
pOut->pqr[_Z] = _body2imu[_Z][_X] * pIn->cg_pqr[_X] + _body2imu[_Z][_Y] * pIn->cg_pqr[_Y] + _body2imu[_Z][_Z] * pIn->cg_pqr[_Z];
// calculate IMU attitude (DCM matrix)
_matTemp1[_X][_X] = _body2imu[_X][_X] * _earth2body[_X][_X] + _body2imu[_X][_Y] * _earth2body[_Y][_X] + _body2imu[_X][_Z] * _earth2body[_Z][_X];
_matTemp1[_X][_Y] = _body2imu[_X][_X] * _earth2body[_X][_Y] + _body2imu[_X][_Y] * _earth2body[_Y][_Y] + _body2imu[_X][_Z] * _earth2body[_Z][_Y];
_matTemp1[_X][_Z] = _body2imu[_X][_X] * _earth2body[_X][_Z] + _body2imu[_X][_Y] * _earth2body[_Y][_Z] + _body2imu[_X][_Z] * _earth2body[_Z][_Z];
_matTemp1[_Y][_X] = _body2imu[_Y][_X] * _earth2body[_X][_X] + _body2imu[_Y][_Y] * _earth2body[_Y][_X] + _body2imu[_Y][_Z] * _earth2body[_Z][_X];
_matTemp1[_Y][_Y] = _body2imu[_Y][_X] * _earth2body[_X][_Y] + _body2imu[_Y][_Y] * _earth2body[_Y][_Y] + _body2imu[_Y][_Z] * _earth2body[_Z][_Y];
_matTemp1[_Y][_Z] = _body2imu[_Y][_X] * _earth2body[_X][_Z] + _body2imu[_Y][_Y] * _earth2body[_Y][_Z] + _body2imu[_Y][_Z] * _earth2body[_Z][_Z];
_matTemp1[_Z][_X] = _body2imu[_Z][_X] * _earth2body[_X][_X] + _body2imu[_Z][_Y] * _earth2body[_Y][_X] + _body2imu[_Z][_Z] * _earth2body[_Z][_X];
_matTemp1[_Z][_Y] = _body2imu[_Z][_X] * _earth2body[_X][_Y] + _body2imu[_Z][_Y] * _earth2body[_Y][_Y] + _body2imu[_Z][_Z] * _earth2body[_Z][_Y];
_matTemp1[_Z][_Z] = _body2imu[_Z][_X] * _earth2body[_X][_Z] + _body2imu[_Z][_Y] * _earth2body[_Y][_Z] + _body2imu[_Z][_Z] * _earth2body[_Z][_Z];
pOut->THETA[_PHI] = M_ATAN2(_matTemp1[_Y][_Z], _matTemp1[_Z][_Z]);
pOut->THETA[_THETA] = -asin(_matTemp1[_X][_Z]);
pOut->THETA[_PSI] = M_ATAN2(_matTemp1[_X][_Y], _matTemp1[_X][_X]);
}
CFLOAT
M_DECL_FUNC (__clog) (CFLOAT x)
{
CFLOAT result;
int rcls = fpclassify (__real__ x);
int icls = fpclassify (__imag__ x);
if (__glibc_unlikely (rcls == FP_ZERO && icls == FP_ZERO))
{
/* Real and imaginary part are 0.0. */
__imag__ result = signbit (__real__ x) ? (FLOAT) M_MLIT (M_PI) : 0;
__imag__ result = M_COPYSIGN (__imag__ result, __imag__ x);
/* Yes, the following line raises an exception. */
__real__ result = -1 / M_FABS (__real__ x);
}
else if (__glibc_likely (rcls != FP_NAN && icls != FP_NAN))
{
/* Neither real nor imaginary part is NaN. */
FLOAT absx = M_FABS (__real__ x), absy = M_FABS (__imag__ x);
int scale = 0;
if (absx < absy)
{
FLOAT t = absx;
absx = absy;
absy = t;
}
if (absx > M_MAX / 2)
{
scale = -1;
absx = M_SCALBN (absx, scale);
absy = (absy >= M_MIN * 2 ? M_SCALBN (absy, scale) : 0);
}
else if (absx < M_MIN && absy < M_MIN)
{
scale = M_MANT_DIG;
absx = M_SCALBN (absx, scale);
absy = M_SCALBN (absy, scale);
}
if (absx == 1 && scale == 0)
{
__real__ result = M_LOG1P (absy * absy) / 2;
math_check_force_underflow_nonneg (__real__ result);
}
else if (absx > 1 && absx < 2 && absy < 1 && scale == 0)
{
FLOAT d2m1 = (absx - 1) * (absx + 1);
if (absy >= M_EPSILON)
d2m1 += absy * absy;
__real__ result = M_LOG1P (d2m1) / 2;
}
else if (absx < 1
&& absx >= M_LIT (0.5)
&& absy < M_EPSILON / 2
&& scale == 0)
{
FLOAT d2m1 = (absx - 1) * (absx + 1);
__real__ result = M_LOG1P (d2m1) / 2;
}
else if (absx < 1
&& absx >= M_LIT (0.5)
&& scale == 0
&& absx * absx + absy * absy >= M_LIT (0.5))
{
FLOAT d2m1 = M_SUF (__x2y2m1) (absx, absy);
__real__ result = M_LOG1P (d2m1) / 2;
}
else
{
FLOAT d = M_HYPOT (absx, absy);
__real__ result = M_LOG (d) - scale * (FLOAT) M_MLIT (M_LN2);
}
__imag__ result = M_ATAN2 (__imag__ x, __real__ x);
}
else
{
__imag__ result = M_NAN;
if (rcls == FP_INFINITE || icls == FP_INFINITE)
/* Real or imaginary part is infinite. */
__real__ result = M_HUGE_VAL;
else
__real__ result = M_NAN;
}
return result;
}
