;************************************************************ ;IEEE 64 Bit Floating Point Library (c) 2003 M.Cibulski ; ;Transcend Functions ; ;************************************************************ ;************************************************************ ;Inverse Tangent ; ;Parameters: ;Akku A ; ;Registers: ;AKKUA floating point akkumulator ;AKKUB temporary for RAM variable ; ; arctan(x) = x + x^3 P(x^2)/Q(x^2) ; 0 <= x <= 0.66 ;************************************************************ .CSEG flt_atan_p: .db 0xBF,0xEC,0x00,0x7F,0xA1,0xF7,0x25,0x94 ;-8.750608600031904122785E-1, .db 0xC0,0x30,0x28,0x54,0x5B,0x6B,0x80,0x7A ;-1.615753718733365076637E1, .db 0xC0,0x52,0xC0,0x8C,0x36,0x88,0x02,0x73 ;-7.500855792314704667340E1 .db 0xC0,0x5E,0xB8,0xBF,0x2D,0x05,0xBA,0x25 ;-1.228866684490136173410E2 .db 0xC0,0x50,0x36,0x69,0xFD,0x28,0xEC,0x8E ;-6.485021904942025371773E1 .db 0xFF,0xFF flt_atan_q: .db 0x3F,0xF0,0x00,0x00,0x00,0x00,0x00,0x00 ;1.000000000000000000000E0 .db 0x40,0x38,0xDB,0xC4,0x5B,0x14,0x60,0x3C ;2.485846490142306297962E1 .db 0x40,0x64,0xA0,0xDD,0x43,0xB8,0xFA,0x25 ;1.650270098316988542046E2 .db 0x40,0x7B,0x0E,0x18,0xD2,0xE2,0xBE,0x3B ;4.328810604912902668951E2 .db 0x40,0x7E,0x56,0x3F,0x13,0xB0,0x49,0xEA ;4.853903996359136964868E2 .db 0x40,0x68,0x51,0x9E,0xFB,0xBD,0x62,0xEC ;1.945506571482613964425E2 .db 0xFF,0xFF flt_atan_t3p8: .db 0x40,0x03,0x50,0x4F,0x33,0x3F,0x9D,0xE6 ;2.41421356237309504880 flt_atan_066: .db 0x3F,0xE5,0x1E,0xB8,0x51,0xEB,0x85,0x1F ;0.66 flt_atan: .EQU flt_atan_x = 1 .EQU flt_atan_y = 9 .EQU flt_atan_z = 17 .EQU flt_atan_save = 25 .EQU flt_atan_sign = 33 .EQU flt_atan_flag = 34 .EQU flt_atan_lspace = 35 LOCAL flt_atan_lspace push zl push zh f_isnumber r16 ,flt_atan_01 rjmp flt_atan_end flt_atan_01: std Y+flt_atan_sign ,AKKU_S ;save sign clr AKKU_S ;make x positive f_store_l flt_atan_x f_push f_const flt_atan_t3p8 ;x > T3P8 ? f_x_lt_y brtc flt_atan_02 ;no, check for x <= 0.66 f_const flt__pi2 ;yes, y = PI/2; f_store_l flt_atan_y f_const flt__minus1 ; x = -( 1.0/x ); f_xy f_div f_store_l flt_atan_x ldi r16 ,1 ; flag = 1; std Y+flt_atan_flag ,r16 rjmp flt_atan_calc flt_atan_02: f_const flt_atan_066 ;x <= 0.66 ? f_x_ge_y brtc flt_atan_03 ;no f_zero ;yes, y = 0.0; f_store_l flt_atan_y f_pop ;get x clr r16 ;flag = 0; std Y+flt_atan_flag ,r16 rjmp flt_atan_calc flt_atan_03: f_const flt__pi4 ;no, y = PI/4; f_store_l flt_atan_y f_const flt__1 ; x = (x-1)/(x+1); f_sub f_push f_load_l flt_atan_x f_push f_const flt__1 ; x = (x-1)/(x+1); f_add f_div f_store_l flt_atan_x ldi r16 ,2 ; flag = 2; std Y+flt_atan_flag ,r16 flt_atan_calc: f_push ;z = x * x; f_mul f_store_l flt_atan_z f_push f_powerseries flt_atan_p ;z = z * powerseries (z, P) / powerseries (z, Q); f_mul f_push f_load_l flt_atan_z f_powerseries flt_atan_q f_div f_push ;z = x * z + x; f_load_l flt_atan_x f_mul f_push f_load_l flt_atan_x f_add f_store_l flt_atan_z f_push ;y = y + z f_load_l flt_atan_y f_add ldd r0 ,Y+flt_atan_sign ;sign of x to y eor AKKU_S ,r0 flt_atan_end: pop zh pop zl ENDLOCAL flt_atan_lspace ret ;} ;************************************************************ ;Inverse Tangent of X, Y ; ;Parameters: ;Y Y ;AKKU X ; ;************************************************************ .CSEG flt_atan2: .EQU flt_atan2_x = 1 .EQU flt_atan2_y = 9 .EQU flt_atan2_quad = 17 .EQU flt_atan2_ex1 = 18 .EQU flt_atan2_ex2 = 19 .EQU flt_atan2_lspace = 20 LOCAL flt_atan2_lspace push zl push zh f_isnumber r16 ,flt_atan2_01 rjmp flt_atan2_end flt_atan2_01: f_store_l flt_atan2_x asr AKKU_S std Y+flt_atan2_quad ,AKKU_S std Y+flt_atan2_ex1 ,AKKU_E1 std Y+flt_atan2_ex2 ,AKKU_E2 ldd AKKU_S ,Y+flt_atan2_lspace+FLT_Y_S ldd AKKU_E1 ,Y+flt_atan2_lspace+FLT_Y_E1 ldd AKKU_E2 ,Y+flt_atan2_lspace+FLT_Y_E2 ldd AKKU_2 ,Y+flt_atan2_lspace+FLT_Y_2 ldd AKKU_3 ,Y+flt_atan2_lspace+FLT_Y_3 ldd AKKU_4 ,Y+flt_atan2_lspace+FLT_Y_4 ldd AKKU_5 ,Y+flt_atan2_lspace+FLT_Y_5 ldd AKKU_6 ,Y+flt_atan2_lspace+FLT_Y_6 ldd AKKU_7 ,Y+flt_atan2_lspace+FLT_Y_7 ldd AKKU_8 ,Y+flt_atan2_lspace+FLT_Y_8 f_isnumber r16 ,flt_atan2_02 rjmp flt_atan2_end ;yes flt_atan2_02: f_store_l flt_atan2_y ldd r0 ,Y+flt_atan2_quad or r0 ,AKKU_S std Y+flt_atan2_quad ,r0 flt_atan2_no_nan: ldd r16 ,Y+flt_atan2_ex2 ;exp(X) ldd r17 ,Y+flt_atan2_ex1 mov r20 ,AKKU_E2 mov r21 ,AKKU_E1 sub r20 ,r16 ;if( fabs(x) <= (fabs(y) / MAXNUM) ) { sbc r21 ,r17 ;(exponentY - exponentX > 60) brcs flt_atan2_chk_y_zero ldi r16 ,low(60) ldi r17 ,high(60) cp r20 ,r16 cpc r21 ,r17 brcs flt_atan2_chk_y_zero tst AKKU_S ;if (Y < 0.0) { brpl flt_atan2_no_3pi2 ; return( -PI/2 ); f_const flt__minuspi2 rjmp flt_atan2_end ;} flt_atan2_no_3pi2: tst AKKU_E2 ;if( y == 0.0 ) { brne flt_atan2_y_not_null tst AKKU_E1 brne flt_atan2_y_not_null flt_atan2_null: rjmp flt_atan2_end ; return( 0.0 ); ;} flt_atan2_y_not_null: ;else { f_const flt__pi2 ; return( PIO2 ); rjmp flt_atan2_end ;} flt_atan2_chk_y_zero: tst AKKU_E1 ;if( y == 0.0 ) { brne flt_atan2_calc tst AKKU_E2 brne flt_atan2_calc ldd r16 ,Y+flt_atan2_quad andi r16 ,0b01000000 ;if( X >= 0.0 ) { brne flt_atan2_pi ; return 0.0 f_zero rjmp flt_atan2_end flt_atan2_pi: f_const flt__pi ;} else return( PI ); rjmp flt_atan2_end ;} flt_atan2_calc: f_push f_load_l flt_atan2_x call flt_div ;Y/X call flt_atan ldd r16 ,Y+flt_atan2_quad ;Quadrant cpi r16 ,0xC0 ;Y<0 , X<0 breq flt_atan2_c0 cpi r16 ,0x80 ;Y<0 breq flt_atan2_80 cpi r16 ,0x40 ;X<0 breq flt_atan2_40 rjmp flt_atan2_00 ;X,Y > 0 flt_atan2_40: f_push f_const flt__pi f_add rjmp flt_atan2_end flt_atan2_c0: f_push f_const flt__pi f_sub rjmp flt_atan2_end flt_atan2_80: f_push f_const flt__pi f_add rjmp flt_atan2_end flt_atan2_00: flt_atan2_end: pop zh pop zl ENDLOCAL flt_atan2_lspace pop r16 ;save return adress pop r17 pop r0 ;remove FLT_Y from stack pop r0 pop r0 pop r0 pop r0 pop r0 pop r0 pop r0 pop r0 pop r0 push r17 ;restore return adress push r16 ret