/*
* ATAN2 - pass by value for real(kind=4)
*/
_f_real4
_HATAN2( _f_real4 x,
_f_real4 y)
{
_f_real8 __atan2(_f_real8 x, _f_real8 y);
return ((_f_real4) __atan2((_f_real8) x, (_f_real8) y));
}
/*
* complex(kind=8) raised to a real(kind=8) = _CTOR
*
* x = a+b*i
*
* if ((x == 0+0*i) && (y == 0)) then return(NAN)
* if (x == 0+0*i) then return(0+0*i)
* if (y == 0) then return(1+0*i)
*/
void
_CTOR(c_complex_t *ret_val,
c_complex_t x,
_f_real8 *r)
{
_f_real8 __atan2(_f_real8 x, _f_real8 y);
_f_real8 __cos(_f_real8 x);
_f_real8 __exp(_f_real8 x);
_f_real8 __log(_f_real8 x);
_f_real8 __sin(_f_real8 x);
_f_real8 _CABS(c_complex_t z);
_f_real8 y = *r;
_f_real8 one;
_f_real8 two;
if (x.real == (_f_real8) 0.0 && x.imag == (_f_real8) 0.0) {
if (y == (_f_real8) 0.0) {
ret_val->real = _SGL_NaN;
ret_val->imag = _SGL_NaN;
}
else {
ret_val->real = (_f_real8) 0.0;
ret_val->imag = (_f_real8) 0.0;
}
return;
}
one = y * __atan2(x.imag, x.real);
two = y * __log(_CABS(x));
ret_val->real = __exp(two) * __cos(one);
ret_val->imag = __exp(two) * __sin(one);
}
__declspec ( naked ) void nseel_asm_atan2(void)
{
FUNC2_ENTER
*__nextBlock = __atan2(*parm_b, *parm_a);
FUNC_LEAVE
}
void filterIMUdata(char acc[], char gyro[], char compass[], int *pitch, int *roll, int *yaw){
int pitchAcc, rollAcc, yawMag;
*pitch += 100 * gyro[0] * 3.906 * dt; // Angle around the X-axis
*roll -= 100 * gyro[1] * 3.906 * dt; // Angle around the Y-axis
*yaw += 100 * gyro[2] * 3.906 * dt;
int forceMagnitudeApprox = abs(acc[0]) + abs(acc[1]) + abs(acc[2]);
if (forceMagnitudeApprox > 32 && forceMagnitudeApprox < 128) // 32 - 128 -- 8192 - 32768
{
pitchAcc = 10 * __atan2(acc[0], acc[2]); // 1 2
*pitch = *pitch * compFilterCoef + pitchAcc * (1 - compFilterCoef);
rollAcc = 10 * __atan2(-acc[1], acc[2]); //-0 2
*roll = *roll * compFilterCoef + rollAcc * (1 - compFilterCoef);
}
yawMag = 10 * __atan2(compass[0],compass[1]);
*yaw = *yaw * compFilterCoef + yawMag * (1 - compFilterCoef);
}
double
__carg (__complex__ double x)
{
return __atan2 (__imag__ x, __real__ x);
}
double atan2(double y, double x) {
return __atan2( y, x );
}
