/* * 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 ); }