;***********************************************************************

;* Code Utilities: Copyright © 2002 Alpha Omega Media, Inc. All Rights Reserved.

;***********************************************************************

; registers R26:R27 (X), R28:29 (Y) and R30:31 (Z) reserved as mem pointers

.equ RemainderInit = 0x40000000 ; initial remainder for Root()

.equ DividendInit = 0x60000000 ; initial dividend for Ratio()

; SQ[] holds the square from the AC volt readings, and the divisor for ohms

.def SQ0 = R10 ; Low Byte, Word 0 of square

.def SQ1 = R11 ; Hi Byte, Word 0 of square

.def SQ2 = R12 ; Low Byte, Word 1 of square

.def SQ3 = R13 ; Hi Byte, Word 1 of square

; General variables - Temp0 and Temp1 are maintained across interrupts

.def Temp0 = R16 ; Temporary storage register; Global

.def Temp1 = R17 ; General Purpose Global variable

.def Temp2 = R18 ; Used in Sqrt

.def Temp3 = R19 ; Used in Sqrt

;***********************************************************************

;* Ratio: - Divides 32-bit Temp[] (dividend) by 32-bit variable in SQ[] (divisor)

;* DD = dividend, VR = divisor, RM = remainder. SQ[] (divisor) is stored in

;* SRAM and recalled as needed. SQ[] is then used as initial dividend.

;* result is shifted into the dividend SQ[], which returns the final answer.

;* mem1, mem2 are pointers to 4-byte memory words.

;* Author: Frank Raffaeli – AOM, Inc. Copyright 2002

;***********************************************************************

Ratio: ; start here for arbitrary dividend

ldi XL, low(mem1) ; load pointer low byte mem1

ldi XH, high(mem1) ; load pointer high byte mem1

rcall SQ_2_Mem ; store SQ[] (input) into mem1

movw SQ0, Temp0 ; move low word of DD into SQ[]

movw SQ2, Temp2 ; move hi word of DD into SQ[]

clr Temp0 ; init Temp[] with low byte: RM = 0

clr Temp1 ; init Temp[] with 2nd byte: RM = 0

clr Temp2 ; init Temp[] with 3rd byte: RM = 0

sub Temp3, Temp3 ; init 4th byte: RM=0 & clear carry

ldi EE_Addr, 33 ; moose use of EE_Addr as counter

divide_loop1:

rol SQ0 ; shift dividend left

rol SQ1 ; carry into byte 2 of dividend

rol SQ2 ; carry into byte 3 of dividend

rol SQ3 ; carry into byte 4 of dividend

dec EE_Addr ; decrement moose counter

breq ExitRatio ; bug out when moose says

rol Temp0 ; shift dividend into remainder

rol Temp1 ; shift RM byte 2

rol Temp2 ; shift RM byte 3

rol Temp3 ; shift RM byte 4

ldi XL, low(mem2) ; load pointer low byte mem2

ldi XH, high(mem2) ; load pointer high byte mem2

rcall SQ_2_Mem ; store SQ[] (DD/result) to mem2

ldi XL, low(mem1) ; load pointer low byte mem1

ldi XH, high(mem1) ; load pointer high byte mem1

rcall LoadSQ ; get divisor from mem1 into SQ[]

sub Temp0, SQ0 ; RM = RM - VR (low byte)

sbc Temp1, SQ1 ; RM = RM - VR (carry 2nd byte)

sbc Temp2, SQ2 ; RM = RM - VR (carry 3rd byte)

sbc Temp3, SQ3 ; RM = RM - VR (carry 4th byte)

brcs add_back ; if carry set, add back to RM

ldi XL, low(mem2) ; load pointer low byte mem2

ldi XH, high(mem2) ; load pointer high byte mem2

rcall LoadSQ ; get DD/result from mem2 into SQ[]

sec ; set carry, shift '1' into DD

rjmp divide_loop1

add_back:

add Temp0, SQ0 ; RM = RM + VR (restoreremainder)

adc Temp1, SQ1 ; RM = RM + VR (carry 2nd byte)

adc Temp2, SQ2 ; RM = RM + VR (carry 3rd byte)

adc Temp3, SQ3 ; RM = RM + VR (carry 4th byte)

ldi XL, low(mem2) ; load pointer low byte mem2

ldi XH, high(mem2) ; load pointer high byte mem2

rcall LoadSQ ; get DD/result from mem2 into SQ[]

clc ; clear carry, shift '0' into DD

rjmp divide_loop1 ; back to start

ExitRatio:

; rcall RatioFilter ; smooth the data in SQ[] (optional)

; sbr Status, (1<<DataReady) ; data ready for CalcDisplayOhms()

ret

;***********************************************************************

;*

;* Root: - Takes the square root of a 32-bit integer

;*

;* -- This is a square root algorithm for unsigned 32 bit integers that uses

;* -- addition, subtraction, and shifts only. Result is truncated: Root(62) = 7

;* -- For a description of the algorithm, see the German computer magazine

;* -- c't, January 1990, page 300 ff. (Otto Peter, "Prozessor zieht Wurzeln")

;*

;* Author: Frank Raffaeli August 2002, AOM, Inc.

;*

;* Root : Unsigned_32 := 0; * Square root

;* RM : Unsigned_32 := 16#4000_0000#; * Remainder

;* X1 : Unsigned_32 := X; * Input, Passed in from SQ[]

;* X2 : Unsigned_32; * Interim 32-bit variable

;*

;* begin

;* loop

;* X2 := Root + RM;

;* Root := Shift_Right (Root, 1);

;* if X2 <= X1 then

;* X1 := X1 - X2;

;* Root := Root + RM;

;* end if;

;* RM := Shift_Right (RM, 2);

;* exit when RM = 0;

;* end loop;

;* return Root;

;* end Sqrt;

;*

;* In order to save on register usage, the functions below are used:

;*

;* Temp_2_Mem -> Saves contents of 32-bit Temp[] register to SRAM

;* SQ_2_Mem -> Saves contents of 32-bit SQ[] register to SRAM

;* LoadTemp -> Loads 32-bit Temp[] register with contents of SRAM

;* LoadSQ -> Loads 32-bit SQ[] register with contents of SRAM

;*

;* The modified algorithm is:

;*

;* Store X1 (SQ[]) into Mem1 * Save input data in Mem1

;* Load immediate constant 0x40000000 into Temp[] * Remainder init

;* clear SQ[] * Root register (for now)

;* Store Temp[] into Mem2 * Save remainder in Mem2

;*

;* root_loop:

;* Temp[] (RM) <- Temp + SQ[] * Temp[] = X2

;* SQ[] <- (SQ >> 1) * Shift Root right

;* Store SQ[] into Mem3 * Save root in Mem3

;* Load X1 from Mem1 into SQ[] * Ld X1 into SQ[] fm Mem1

;* Compare SQ[] (X1) to Temp[] (X2) * is X1 >= X2 ?

;*

;* brlo get_RM (else) * conditional branch

;* X1 (SQ[]) <- X1 - X2 (Temp[]) * subtract X2 from X1

;* Store X1 (SQ[]) into Mem1 * Store X1 into Mem1

;* Load Root into SQ[] from Mem3 * SQ[] <- Root from Mem3

;* Load Remainder into Temp[] from Mem2 * Temp <- RM from Mem2

;* SQ[] (Root) = Root + Temp[] (RM) * Root = Root+RM

;* rjmp update_RM * skip over to update

;*

;* get_RM:

;* Load Temp[] with RM from Mem2 * get remainder from Mem2

;* Load SQ[] with Root from Mem3 * get Root from Mem3

;*

;* update_RM:

;* Temp[] <- Temp[] >> 2 * RM >> 2

;* Store Temp[] into Mem2 * Save remainder in Mem2

;* brne root_loop * if RM !=0, go back

;*

;***********************************************************************

Root:

cbr Status, (1<<ACC_Ready) ; clear accumulator ready flag

ldi XL, low(mem1) ; load pointer low byte mem1

ldi XH, high(mem1) ; load pointer high byte mem1

rcall SQ_2_Mem ; store SQ[] (input) into mem1

ldi Temp0, low(RemainderInit) ; initialize low byte of 32-bit RM

ldi Temp1, high(RemainderInit) ; initialize 2nd byte of 32-bit RM

ldi Temp2, byte3(RemainderInit) ; initialize 3rd byte of 32-bit RM

ldi Temp3, byte4(RemainderInit) ; initialize 4th byte of 32-bit RM

clr SQ0

clr SQ1

movw SQ2, SQ0 ; shortcut clear of SQ[]

ldi XL, low(mem2) ; load pointer low byte mem2

ldi XH, high(mem2) ; load pointer high byte mem2

rcall Temp_2_Mem ; store Temp[] (RM) into mem2

root_loop:

add Temp0, SQ0 ; add low bytes

adc Temp1, SQ1 ; add 2nd byte with carry

adc Temp2, SQ2 ; add 3rd byte with carry

adc Temp3, SQ3 ; finished: Temp[] = Temp[] + SQ[]

lsr SQ3 ; begin at 4th byte, SQ[] >> 1

ror SQ2 ; 3rd byte >> 1 with carry

ror SQ1 ; 2nd byte >> 1 with carry

ror SQ0 ; finished SQ[] (Root) >> 1

ldi XL, low(mem3) ; load pointer low byte mem3

ldi XH, high(mem3) ; load pointer high byte mem3

rcall SQ_2_Mem ; store SQ[] (root) into mem3

ldi XL, low(mem1) ; load pointer low byte mem1

ldi XH, high(mem1) ; load pointer high byte mem1

rcall LoadSQ ; mem1 (input) -> SQ[]

cp SQ0, Temp0 ; compare low byte X1 to X2

cpc SQ1, Temp1 ; compare 2nd byte with carry

cpc SQ2, Temp2 ; compare 3rd byte with carry

cpc SQ3, Temp3 ; is SQ[] (X1) >= Temp[] (X2) ?

brlo get_RM ; if not, get remainder, else cont

sub SQ0, Temp0 ; subtract low bytes (X1 = X1 - X2)

sbc SQ1, Temp1 ; subtract 2nd byte with carry

sbc SQ2, Temp2 ; subtract 3rd byte with carry

sbc SQ3, Temp3 ; finished: SQ[] = SQ[] - Temp[]

ldi XL, low(mem1) ; load pointer low byte mem1

ldi XH, high(mem1) ; load pointer high byte mem1

rcall SQ_2_Mem ; store SQ[] (X1) into mem1

ldi XL, low(mem3) ; load pointer low byte mem3

ldi XH, high(mem3) ; load pointer high byte mem3

rcall LoadSQ ; mem3 (root) -> SQ[]

ldi XL, low(mem2) ; load pointer low byte mem2

ldi XH, high(mem2) ; load pointer high byte mem2

rcall LoadTemp ; mem2 (remainder : RM) -> Temp[]

add SQ0, Temp0 ; add low bytes Root = Root + RM

adc SQ1, Temp1 ; add 2nd byte with carry

adc SQ2, Temp2 ; add 3rd byte with carry

adc SQ3, Temp3 ; finished: SQ[] = SQ[] + Temp[]

rjmp update_RM ; skip over RM retreival, it's done

get_RM:

ldi XL, low(mem2) ; load pointer low byte mem2

ldi XH, high(mem2) ; load pointer high byte mem2

rcall LoadTemp ; mem2 (remainder : RM) -> Temp[]

ldi XL, low(mem3) ; load pointer low byte mem3

ldi XH, high(mem3) ; load pointer high byte mem3

rcall LoadSQ ; mem3 (root) -> SQ[]

update_RM:

lsr Temp3 ; begin at 4th byte, Temp[] >> 1

ror Temp2 ; 3rd byte >> 1 with carry

ror Temp1 ; 2nd byte >> 1 with carry

ror Temp0 ; Temp (Remainder, RM) >> 1

lsr Temp3 ; do it again, Temp[] >> 1

ror Temp2 ; 3rd byte >> 1 with carry

ror Temp1 ; 2nd byte >> 1 with carry

ror Temp0 ; Temp (Remainder, RM) >> 2

ldi XL, low(mem2) ; load pointer low byte mem2

ldi XH, high(mem2) ; load pointer high byte mem2

rcall Temp_2_Mem ; store Temp[] (RM) into mem2

tst Temp3 ; is it zero?

brne root_loop ; if not, go back to start

tst Temp2 ; is it zero?

brne root_loop ; if not, go back to start

tst Temp1 ; is it zero?

brne root_loop ; if not, go back to start

tst Temp0 ; is it zero?

brne root_loop ; if not, go back to start

root_done:

sbr Status, (1<<DataReady) ; set data ready for CalcDisplayAC()

ret

;***********************************************************************

;* LoadSQ -> SRAM at X[] address loaded into 32-bit SQ[] array

;* Uses register X[] to index SRAM

;***********************************************************************

LoadSQ:

ld SQ0, X+ ; load 1st SRAM byte into SQ0

ld SQ1, X+ ; load 2nd SRAM byte into SQ1

ld SQ2, X+ ; load 3rd SRAM byte into SQ2

ld SQ3, X+ ; load 4th SRAM byte into SQ3

sbiw XL, width_32 ; return X[] pointer to where it was

ret

;***********************************************************************

;* SQ_2_Mem -> Contents of 32-bit SQ[] -> SRAM at X[] address

;* Uses register X[] to index SRAM

;***********************************************************************

SQ_2_Mem:

st X+, SQ0 ; store SQ0 in 1st byte of SRAM

st X+, SQ1 ; store SQ1 in 2nd byte of SRAM

st X+, SQ2 ; store SQ2 in 3rd byte of SRAM

st X+, SQ3 ; store SQ3 in 4th byte of SRAM

sbiw XL, width_32 ; return X[] pointer to where it was

ret

;***********************************************************************

;* Temp_2_Mem -> Contents of 32-bit Temp[] -> SRAM at X[] address

;* Uses register X[] to index SRAM

;***********************************************************************

Temp_2_Mem:

st X+, Temp0 ; store Temp0 in 1st byte of SRAM

st X+, Temp1 ; store Temp1 in 2nd byte of SRAM

st X+, Temp2 ; store Temp2 in 3rd byte of SRAM

st X+, Temp3 ; store Temp3 in 4th byte of SRAM

sbiw XL, width_32 ; return X[] pointer to where it was

ret

;***********************************************************************

;* LoadTemp -> SRAM at X[] address loaded into 32-bit Temp[] array

;* Uses register X[] to index SRAM

;***********************************************************************

LoadTemp:

ld Temp0, X+ ; load 1st SRAM byte into Temp0

ld Temp1, X+ ; load 2nd SRAM byte into Temp1

ld Temp2, X+ ; load 3rd SRAM byte into Temp2

ld Temp3, X+ ; load 4th SRAM byte into Temp3

sbiw XL, width_32 ; return X[] pointer to where it was

ret