Wurzelfunktion für 16bit Fixed Point Werte

[StefanK (Gast)]

In Beitrag "Wurzefunktion für uint32_t in C" werden verschiedene 
Algorithmen zum Wurzelziehen aus 32bit Werten angeboten.
Insbesondere die Assembler Funktion von Johann L. ist sehr effektiv.

Möchte man die Wurzel nun aus einem Fixed Point Wert U(a,b) mit b 
Nachkommastellen ziehen kann man diese Funktion verwenden:

Pseudocode für b=8

uint16_t fixQ8val = X;         // mit X = float * 2^b
uint16_t res;
res = sqrt32(val);
res = mult_u16_fixQ8(res, Y);  // mit Y = sqrt(2^b)*2^b

Im vorliegenden Sonderfall von b=8 ist eine einfache (nicht fixed point) 
Multiplikation möglich. Da sqrt(2^8)=16 kann man ohne 
Genauigkeitsverlust direkt Multiplizieren.
Die Abarbeitung ist ebenfalls recht flott (sqrt32~400cycle; mul_u16~50).
Der Nachteil hierbei ist die mangelnde Genauigkeit, da die sqrt32 
Funktion den Wert gerundet zurückgibt und u.U. zusätzliche 
Rundungsfehler beim umwandeln der Konstante sqrt(2^b) in fixed point 
Repräsentation entstehen.

Eine genauere Funktion wurde von Turkowski (Turkowski, Ken, Fixed Point 
Square Root, p. 22-24, code: p. 23 - 
http://tog.acm.org/resources/GraphicsGems/) für 32bit fixed point Werte 
veröffentlicht.
Für 16bit fixed Point Werte sieht die Funktion so aus:

int16_t sqrt16_fixQ8(int16_t a)
{
    uint16_t testDiv = 0;
    uint16_t root = 0;
    uint16_t remHi = 0;
    uint16_t remLo = (int16_t)a;
    uint8_t count = 11;  //(15 + b)/2
    do
    {
        remHi =(remHi << 2) | (remLo >> 14);
        remLo <<= 2;
        root <<= 1;
        testDiv = (root << 1) + 1;
        if (remHi >= testDiv)
        {
            remHi -= testDiv;
            root += 1;
        }
    }
    while (count-- != 0);
    return (int16_t)root;
}

Für andere Werte von b muss lediglich die Variable count entsprechend 
initialisiert werden.
Das Ergebnis ist deutlich genauer als das mit vorheriger vorgehensweise 
allerdings braucht der von gcc erstellte Code bei mir ~1700cycles!

Ich habe die Funktion deshalb in Assembler umgesetzt

.global sqrt16_fixQ8
.func sqrt16_fixQ8
;--------------------------------------------------------------------
;  R25:R24 = SQRT (R25:R24) for fixed point values of precision U(a, b) 
;  The precision of U(a, b) determines how many times the loop is gone 
;  through. With b being the number of fractional bits:
;  N = (16+b)/2
;  in this case N = (16+8)/2 = 12
;--------------------------------------------------------------------

#define TESTDIV0  r18
#define TESTDIV1  r19

#define ROOT0    r20
#define ROOT1    r21

#define REMHI0    r22
#define REMHI1    r23

#define REMLO0    r24
#define REMLO1    r25

#define TEMP0    r26

#define CNT      r27

            


sqrt16_fixQ8:           clr REMHI0                       ; Initialize: REMLO=argument(R25:R24); REMHI=0; ROOT=0; 
                        clr REMHI1
                        clr ROOT0
                        clr ROOT1
                        ldi CNT,12                       ; set number of loops N            
sqrt16_fixQ8_loop:      lsl REMHI0                       ; remhi = remhi<<2
                        rol REMHI1
                        lsl REMHI0
                        rol REMHI1

                        mov TEMP0, REMLO1                ; remlo>>14 -> trunk REMLO0
                        lsr TEMP0                        ; still 6 shifts left
                        lsr TEMP0
                        lsr TEMP0
                        lsr TEMP0
                        lsr TEMP0
                        lsr TEMP0

                        or REMHI0, TEMP0                 ; remhi = remhi<<2(which is REMHI1:REMHI0) | remlo>>14(which is 0x00:TEMP0)
                                                         ; REMHI1 stays REMHI1, since or with 0x00

                        lsl REMLO0                       ; remlo <<= 2
                        rol REMLO1
                        lsl REMLO0
                        rol REMLO1

                        lsl ROOT0                        ; root <<= 1
                        rol ROOT1

                        movw TESTDIV0, ROOT0             ; testdiv = (root<<1) + 1
                        lsl TESTDIV0
                        rol TESTDIV1
                        subi TESTDIV0,-1                 ; addi 1
                        sbci TESTDIV1,-1                 ; adci 0

                                                         ; if(remhi >= testdiv)  
                        cp REMHI1, TESTDIV1              ; carry set if REMHI1 < TESTDIV1, Z set if REMHI1 == TESTDIV1
                        breq sqrt16_fixQ8_test_low       ; when Z is set test low byte
                        brsh sqrt16_fixQ8_same_higher    ; when carry is not set REMHI1 >= TESTDIV, 
                                                         ; the case == is covered above so reaching this means REMHI1 > TESTDIV
                        rjmp sqrt16_fixQ8_lower          ; when carry was set REMHI1 is smaller

sqrt16_fixQ8_test_low:  cp REMHI0, TESTDIV0              ; carry is set if REMHI0 < TESTDIV0
                        brlo sqrt16_fixQ8_lower          ; leave if carry was set
                        rjmp sqrt16_fixQ8_same_higher    ; when carry was not set REMHI0 >= TESTDIV0

sqrt16_fixQ8_same_higher:  sub REMHI0, TESTDIV0          ; remhi -= testdiv
                           sbc REMHI1, TESTDIV1

                           subi ROOT0,-1                 ; root += 1
                           sbci ROOT1,-1
              

sqrt16_fixQ8_lower:        dec CNT
                           brne sqrt16_fixQ8_loop

                           movw r24, ROOT0               ; store result in R25:R24
                           ret

#undef TESTDIV0  
#undef TESTDIV1  

#undef ROOT0  
#undef ROOT1  

#undef REMLO0  
#undef REMLO1  

#undef REMHI0  
#undef REMHI1  

#undef TEMP0  
#undef TEMP1  

#undef CNT    

.endfunc

Sie läuft mit ~500cycle fast so schnell wie die zuerst genannte 
Möglichkeit bei höherer Genauigkeit.
Ich hoffe die Funktion ist für jemanden nützlich. Über 
Verbesserungsvorschläge im Assemblercode oder Anmerkungen zu versteckten 
Fehlern (bin Assembler Neuling) würde ich mich freuen.

Viele Grüße

[Johann L. (gjlayde)]

StefanK schrieb:
> In Beitrag "Wurzefunktion für uint32_t in C" werden verschiedene
> Algorithmen zum Wurzelziehen aus 32bit Werten angeboten.
> Insbesondere die Assembler Funktion von Johann L. ist sehr effektiv.

Die Implementierung ist von Ruud v. Gessel, ich hab sie nur 
avr-gcc-tauglich gemacht.

http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29

> Möchte man die Wurzel nun aus einem Fixed Point Wert U(a,b) mit b
> Nachkommastellen ziehen kann man diese Funktion verwenden:
>
> Pseudocode für b=8
>

> uint16_t fixQ8val = X;         // mit X = float * 2^b
> uint16_t res;
> res = sqrt32(val);
> res = mult_u16_fixQ8(res, Y);  // mit Y = sqrt(2^b)*2^b
>

Spricht bei dir was gegen 4^b? Dass ist einfacher und zudem ohne Fehler 
zu wurzeln.

> Im vorliegenden Sonderfall von b=8 ist eine einfache (nicht fixed point)
> Multiplikation möglich. Da sqrt(2^8)=16 kann man ohne
> Genauigkeitsverlust direkt Multiplizieren.
> Die Abarbeitung ist ebenfalls recht flott (sqrt32~400cycle; mul_u16~50).

mul_u16~50??? Doch hoffentlich auf einer Maschine ohne MUL*.

> Der Nachteil hierbei ist die mangelnde Genauigkeit, da die sqrt32
> Funktion den Wert gerundet zurückgibt und u.U. zusätzliche
> Rundungsfehler beim umwandeln der Konstante sqrt(2^b) in fixed point
> Repräsentation entstehen.
>
> Eine genauere Funktion wurde von Turkowski (Turkowski, Ken, Fixed Point
> Square Root, p. 22-24, code: p. 23 -
> http://tog.acm.org/resources/GraphicsGems/) für 32bit fixed point Werte
> veröffentlicht.
> Für 16bit fixed Point Werte sieht die Funktion so aus:
>
> [c]
> int16_t sqrt16_fixQ8(int16_t a)
> {
>     uint16_t testDiv = 0;
>     uint16_t root = 0;
>     uint16_t remHi = 0;
>     uint16_t remLo = (int16_t)a;
>     uint8_t count = 11;  //(15 + b)/2
>     do
>     {
>         remHi =(remHi << 2) | (remLo >> 14);
>         remLo <<= 2;

Das geht wesentlich schmerzfreier mit

LSL  remLo.lo
ROL  remLo.hi
ROL  remHi.lo
ROL  remHi.hi

LSL  remLo.lo
ROL  remLo.hi
ROL  remHi.lo
ROL  remHi.hi

>         root <<= 1;
>         testDiv = (root << 1) + 1;

Das ist schlicht (da Bit 0 vor der Addition = 0 ist):

SEC
ROL  root.lo
ROL  root.hi
;; oder
LSL  root.lo
ROL  root.hi
INC  root.lo

Ich hab nicht auf's Interface geschaut, ist das avr-gcc ABI?
Wär sinnvoll...

[StefanK (Gast)]

Johann L. schrieb:
> Spricht bei dir was gegen 4^b? Dass ist einfacher und zudem ohne Fehler
> zu wurzeln.

Dabei kann ich dir nicht ganz folgen, was meinst du damit?

Johann L. schrieb:
>mul_u16~50??? Doch hoffentlich auf einer Maschine ohne MUL*.

mh ne ist mit mul, aber ich ich seh grad das ich in meinem test die 
signed multiplikation erwischt habe.

Johann L. schrieb:
> LSL  remLo.lo
> ROL  remLo.hi
> ROL  remHi.lo
> ROL  remHi.hi
>
> LSL  remLo.lo
> ROL  remLo.hi
> ROL  remHi.lo
> ROL  remHi.hi

Da kann ich dir wieder nicht folgen.. kann ich das so in den C code 
reinschreiben?

Johann L. schrieb:
> Ich hab nicht auf's Interface geschaut, ist das avr-gcc ABI?
> Wär sinnvoll...

Als Neuling kann ich damit auch net so viel anfangen... ich hab 
AVRStudio 5 installiert.

Viele Grüße

[StefanK (Gast)]

StefanK schrieb:
> Johann L. schrieb:
>> LSL  remLo.lo
>> ROL  remLo.hi
>> ROL  remHi.lo
>> ROL  remHi.hi
>>
>> LSL  remLo.lo
>> ROL  remLo.hi
>> ROL  remHi.lo
>> ROL  remHi.hi

Ach du meinst im Assember code... jetzt seh ichs...
das ist ja tatsächlich deutlich angenehmer. Danke!

Wenn ich sowas auch nur mal sehen würde... bei mir werden das immer ganz 
wirre konstrukte

[Johann L. (gjlayde)]

StefanK schrieb:
> Johann L. schrieb:
>> Spricht bei dir was gegen 4^b? Dass ist einfacher und zudem ohne Fehler
>> zu wurzeln.
>
> Dabei kann ich dir nicht ganz folgen, was meinst du damit?

Oben schreibst du was von Rundingsfehlern bei der Darstellung von

$$\sqrt{2^b}$$

wenn b gerade ist, was bei dir offenbar so ist, dann schreibt sich b als 
b = 2·b' und also

$$\sqrt{2^b} = \sqrt{4^{b'}} = 2^{b'}$$

Ich sehe nicht, wo's da einen Rundungsfehler hat, denn die Darstellung 
ist im 2er-System. Auch negative b' sind ohne Rundungsfehler 
darzustellen. Bei der Division (ein Links-Shift, da b' < 0) gibt's auch 
keine Rundungsfehler.

Freilich verlierst du damit wie korrekt angemerkt Genauigkeit, bzw. sie 
ist durch den 32-Bit-Wurzler garnicht vorhanden.

> Johann L. schrieb:
>>mul_u16~50??? Doch hoffentlich auf einer Maschine ohne MUL*.
>
> mh ne ist mit mul, aber ich ich seh grad das ich in meinem test die
> signed multiplikation erwischt habe.

50 Ticks für ne 16-bit signed-Multiplikation ist immer nocht stolz. Du 
hast Optimierung aktiviert?

> Johann L. schrieb:
>> Ich hab nicht auf's Interface geschaut, ist das avr-gcc ABI?
>> Wär sinnvoll...
>
> Als Neuling kann ich damit auch net so viel anfangen... ich hab
> AVRStudio 5 installiert.

Vielleicht nur eine Fehlinterpretation von mir: Du verwendest ein Mix 
von Assembler und C? Oder ist es ein reines Assembler-Projekt.

Im zweiten Falle ist das ABI egal.
Im ersten Falle will man idR die Assembler-Funktion von C aus aufrufen, 
weil man sich nicht den Wolf machen will das komplette Projekt in asm zu 
klöppeln.  Dann muss allerdings die Aufruf-Konvention von avr-gcc 
eingehalten werden; siehe zB

http://www.rn-wissen.de/index.php/Avr-gcc/Interna#Registerverwendung

[Johann L. (gjlayde)]

StefanK schrieb:

>     uint16_t testDiv = 0;
>     uint16_t root = 0;
>     uint16_t remHi = 0;
>         root <<= 1;
>         if (remHi >= testDiv)
>         {
>             remHi -= testDiv;
>             root += 1;
>         }

Auch diese Stelle ist sehr kompliziert umgesetzt. Das geht:

LSL  root.lo
ROL  root.hi

CP   remHi.lo, testDiv.lo
CPC  remHi.hi, testDiv.hi

BRLO 0f

SUB  remHi.lo, testDiv.lo
SBC  remHi.hi, testDiv.hi
INC  root.Lo

0:

[Johann L. (gjlayde)]

Johann L. schrieb:

> LSL  root.lo
> ROL  root.hi
>
> CP   remHi.lo, testDiv.lo
> CPC  remHi.hi, testDiv.hi
>
> BRLO 0f
>
> SUB  remHi.lo, testDiv.lo
> SBC  remHi.hi, testDiv.hi
> INC  root.lo
>
> 0:

Eine Instruktion kann man da noch sparen wenn ich mich net irre:

LSL  root.lo
ROL  root.hi

SUB  remHi.lo, testDiv.lo
SBC  remHi.hi, testDiv.hi

BRSH 0f

MOVW remHi, testDiv

0:
SBCI  root.lo, -1

Allerdings muss root.lo dann in einem Register >= R16 sein und man 
brauch ein Device mit MOVW. In dem Falle geht dann auch die 
Initialisierung mit 0 kürzer: Anstatt CLR, CLR, CLR, CLR anfangs geht 
CLR, CLR, MOVW.

[StefanK (Gast)]

Johann L. schrieb:
> wenn b gerade ist, was bei dir offenbar so ist, dann schreibt sich b als
> b = 2·b'

Ach, alles klar, das geht - hab ich verstanden. Bleibt das Problem, dass 
sqrt32 immernoch gerundete Werte zurückgibt. Das kann man mit dem o.g. 
Algorithmus umgehen.

Dank deiner Verbesserungen ist er sogar flotter als sqrt32 alleine.

Johann L. schrieb:
> Dann muss allerdings die Aufruf-Konvention von avr-gcc
> eingehalten werden; siehe zB

Ja das beachte ich.


Johann L. schrieb:
> Eine Instruktion kann man da noch sparen wenn ich mich net irre:
> LSL  root.lo
> ROL  root.hi
>
> SUB  remHi.lo, testDiv.lo
> SBC  remHi.hi, testDiv.hi
>
> BRSH 0f
>
> MOVW remHi, testDiv
>
> 0:
> SBCI  root.lo, -1
geht hier nicht, für den fall das remhi<testdiv, mein remhi verloren? 
movw setzt ja einfach testdiv ein, nicht das alte remhi?

Johann L. schrieb:
> 50 Ticks für ne 16-bit signed-Multiplikation ist immer nocht stolz. Du
> hast Optimierung aktiviert?

Nein Optimierung ist nicht aktiv. Die Multiplikation ist allerdings auch 
eine Assembler Routine (32bit = 16bit*16bit). Die Zeit zum Aufrufen 
(Registerzuweisungsgeplänkel von gcc) ist allerdings in den 50 cycles 
dabei.