;************************************************************ ;Square Root ; ;Parameters: ;Akku A ; ;Registers: ;AKKUA floating point akkumulator ;AKKUB temporary for RAM variable ; ; sqrt(x) ;************************************************************ .CSEG flt_sqrt_sqrt2: .db 0x3F,0xF6,0xA0,0x9E,0x66,0x7F,0x3B,0xCD ; 1.4142135623730951 flt_sqrt_approx_b: .db 0x3F,0xDA,0xB5,0x2A,0xEF,0x34,0x8E,0x52 ;4.173075996388649989089E-1 flt_sqrt_approx_a: .db 0x3F,0xE2,0xE2,0x9B,0x8F,0x2F,0xCF,0x79 ;5.9016206709064458299663E-1 flt_sqrt: .equ flt_sqrt_w = 1 ;double w .equ flt_sqrt_x = 9 ;double x .equ flt_sqrt_exp = 17 ;int e .equ flt_sqrt_lspace = 18 ;int e LOCAL flt_sqrt_lspace push zl push zh f_isnumber r16 ,flt_sqrt_01 rjmp flt_sqrt_to_end ;yes, return NAN flt_sqrt_01: tst AKKU_E1 ;AKKU is Zero ? brne flt_sqrt_02 ;no tst AKKU_E2 breq flt_sqrt_to_end ;yes, return Zero flt_sqrt_02: tst AKKU_S ;negative brpl flt_sqrt_03 f_ldiexp r16 ,0x7FF ;sqrt(negative number) = NAN clr AKKU_S clr AKKU_2 clr AKKU_3 clr AKKU_4 clr AKKU_5 clr AKKU_6 clr AKKU_7 clr AKKU_8 flt_sqrt_to_end: rjmp flt_sqrt_end flt_sqrt_03: f_store_l flt_sqrt_w ;w = x; ldi r17 ,0x03 ;store exponent + 1 ldi r16 ,0xfe sub AKKU_E2 ,r16 sbc AKKU_E1 ,r17 std Y+flt_sqrt_exp ,AKKU_E1 std Y+flt_sqrt_exp+1 ,AKKU_E2 f_ldiexp r16 ,0x03FE ;exponent -1 to have 0.5 < x < 1.0 f_push f_const flt_sqrt_approx_a ;x = approx_b + approx_a * x f_mul f_push f_const flt_sqrt_approx_b f_add ldd r17 ,Y+flt_sqrt_exp ;exp = exp >> 1 ldd r16 ,Y+flt_sqrt_exp+1 asr r17 ror r16 add AKKU_E2 ,r16 ;insert exp adc AKKU_E1 ,r17 ldd r16 ,Y+flt_sqrt_exp+1 ;odd exponent ? bst r16 ,0 brtc flt_sqrt_04 f_push f_const flt_sqrt_sqrt2 ;x = x * sqrt(2) f_mul flt_sqrt_04: f_push f_push f_load_l flt_sqrt_w ;x = (w/x + x) / 2 f_xy f_div f_add ;f_subexp r16 ,1 ldi r16 ,low(1) ;exponent -1 sub AKKU_E2 ,r16 ldi r16 ,high(1) sbc AKKU_E1 ,r16 f_push f_push f_load_l flt_sqrt_w ;x = (w/x + x) / 2 f_xy f_div f_add ;f_subexp r16 ,1 ldi r16 ,low(1) ;exponent -1 sub AKKU_E2 ,r16 ldi r16 ,high(1) sbc AKKU_E1 ,r16 f_push f_push f_load_l flt_sqrt_w ;x = (w/x + x) / 2 f_xy f_div f_add ;f_subexp r16 ,1 ldi r16 ,low(1) ;exponent -1 sub AKKU_E2 ,r16 ldi r16 ,high(1) sbc AKKU_E1 ,r16 f_push f_push f_load_l flt_sqrt_w ;x = (w/x + x) / 2 f_xy f_div f_add ;f_subexp r16 ,1 ldi r16 ,low(1) ;exponent -1 sub AKKU_E2 ,r16 ldi r16 ,high(1) sbc AKKU_E1 ,r16 flt_sqrt_end: pop zh pop zl ENDLOCAL flt_sqrt_lspace ret ;}