Subversion Repositories FlightCtrl

;-----------------------------------------------------------------------------;
; Fast integer squareroot routines for avr-gcc project          (C)ChaN, 2008
; http://elm-chan.org/docs/avrlib/sqrt32.S
;-----------------------------------------------------------------------------;
; uint16_t isqrt32 (uint32_t n);
; uint8_t isqrt16 (uint16_t n);
; uint16_t ihypot (int16_t x, int16_t y);

;-----------------------------------------------------------------------------:
; 32bit integer squareroot
;-----------------------------------------------------------------------------;
;   uint16_t isqrt32 (
;     uint32_t n
;   );
;
; Return Value:
;   Squareroot of n.
;
; Size  = 53 words
; Clock = 532..548 cycles
; Stack = 0 byte

.global isqrt32
.func isqrt32

isqrt32:
        clr     r0
        clr     r18
        clr     r19
        clr     r20
        ldi     r21, 1
        clr     r27
        clr     r30
        clr     r31
        ldi     r26, 16
1:      lsl     r22
        rol     r23
        rol     r24
        rol     r25
        rol     r0
        rol     r18
        rol     r19
        rol     r20
        lsl     r22
        rol     r23
        rol     r24
        rol     r25
        rol     r0
        rol     r18
        rol     r19
        rol     r20
        brpl    2f
        add     r0, r21
        adc     r18, r27
        adc     r19, r30
        adc     r20, r31
        rjmp    3f
2:      sub     r0, r21
        sbc     r18, r27
        sbc     r19, r30
        sbc     r20, r31
3:      lsl     r21
        rol     r27
        rol     r30
        andi    r21, 0b11111000
        ori     r21, 0b00000101
        sbrc    r20, 7
        subi    r21, 2
        dec     r26
        brne    1b
        lsr     r30
        ror     r27
        ror     r21
        lsr     r30
        ror     r27
        ror     r21
        mov     r24, r21
        mov     r25, r27
        ret
.endfunc



;-----------------------------------------------------------------------------:
; 16bit integer squareroot
;-----------------------------------------------------------------------------;
;   uint8_t isqrt16 ( uint16_t n );
;
; Return Value:
;   Squareroot of n.
;
; Size  = 33 words
; Clock = 181..189 cycles
; Stack = 0 byte

.global isqrt16
.func isqrt16

isqrt16:
        clr     r18
        clr     r19
        ldi     r20, 1
        clr     r21
        ldi     r22, 8
1:      lsl     r24
        rol     r25
        rol     r18
        rol     r19
        lsl     r24
        rol     r25
        rol     r18
        rol     r19
        brpl    2f
        add     r18, r20
        adc     r19, r21
        rjmp    3f
2:      sub     r18, r20
        sbc     r19, r21
3:      lsl     r20
        rol     r21
        andi    r20, 0b11111000
        ori     r20, 0b00000101
        sbrc    r19, 7
        subi    r20, 2
        dec     r22
        brne    1b
        lsr     r21
        ror     r20
        lsr     r21
        ror     r20
        mov     r24, r20
        ret
.endfunc



;-----------------------------------------------------------------------------:
; 16bit integer hypot (megaAVR is required)
;-----------------------------------------------------------------------------;
;   uint16_t ihypot (
;     int16_t x,
;     int16_t y
;   );
;
; Return Value:
;   Squareroot of (x*x + y*y)
;
; Size  = 42 words
; Clock = 581..597 cycles
; Stack = 0 byte

.global ihypot
.func ihypot

ihypot:
        clr     r26
        sbrs    r25, 7
        rjmp    1f
        com     r24
        com     r25
        adc     r24, r26
        adc     r25, r26
1:      sbrs    r23, 7
        rjmp    2f
        com     r22
        com     r23
        adc     r22, r26
        adc     r23, r26
2:      mul     r22, r22
        movw    r18, r0
        mul     r23, r23
        movw    r20, r0
        mul     r22, r23
        add     r19, r0
        adc     r20, r1
        adc     r21, r26
        add     r19, r0
        adc     r20, r1
        adc     r21, r26
        mul     r24, r24
        movw    r30, r0
        mul     r25, r25
        add     r18, r30
        adc     r19, r31
        adc     r20, r0
        adc     r21, r1
        mul     r24, r25
        add     r19, r0
        adc     r20, r1
        adc     r21, r26
        add     r19, r0
        adc     r20, r1
        adc     r21, r26
        movw    r24, r20
        movw    r22, r18
        clr     r1
        rjmp    isqrt32
.endfunc