--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/compass_ops.asm	Wed Apr 24 19:22:45 2013 +0200
@@ -0,0 +1,171 @@
+#include "ostc3.inc"
+
+; Make sure symbols from the .inc are available to the C code:
+    ; Filtered data
+    global compass_DX_f, compass_DY_f, compass_DZ_f
+    global accel_DX_f,   accel_DY_f,   accel_DZ_f
+
+    ; Calibration data
+    global compass_CX_f
+    global compass_CY_f
+    global compass_CZ_f
+
+    ; Tmp values to pass Q15 arithmetics around
+    global compass_a
+    global compass_b
+
+    ; Result
+    global compass_heading, compass_roll, compass_pitch
+
+compass code
+;-----------------------------------------------------------------------------
+; Filter compass values
+;
+; Apply linear filtering to input parameters.
+    global compass_filter
+
+; Apply filtering formula:
+;   reg_f += (reg - reg_f) / 4
+FILTER16 MACRO   reg, reg_f
+        movf    reg_f+0,W
+        subwf   reg+0,W
+        movwf   PRODL
+        movf    reg_f+1,W
+        subwfb  reg+1,W
+        movwf   PRODH
+
+        bcf     STATUS,C        ; Copy sign bit into carry
+        btfsc   PRODH,7
+        bsf     STATUS,C
+        rrcf    PRODH,F         ; 16bit shift right
+        rrcf    PRODL,F
+
+        bcf     STATUS,C        ; Copy sign bit into carry
+        btfsc   PRODH,7
+        bsf     STATUS,C
+        rrcf    PRODH,F         ; 16bit shift right
+        rrcf    PRODL,W
+
+        addwf   reg_f+0,F
+        movf    PRODH,W
+        addwfc  reg_f+1,F
+        ENDM
+
+compass_filter:
+        banksel   compass_DX
+
+        FILTER16  compass_DX, compass_DX_f
+        FILTER16  compass_DY, compass_DY_f
+        FILTER16  compass_DZ, compass_DZ_f
+        FILTER16  accel_DX,   accel_DX_f
+        FILTER16  accel_DY,   accel_DY_f
+        FILTER16  accel_DZ,   accel_DZ_f
+        banksel   common
+        return
+
+;-----------------------------------------------------------------------------
+
+        global compass_filter_init
+compass_filter_init:
+        movff   compass_DX+0, compass_DX_f+0
+        movff   compass_DX+1, compass_DX_f+1
+        movff   compass_DY+0, compass_DY_f+0
+        movff   compass_DY+1, compass_DY_f+1
+        movff   compass_DZ+0, compass_DZ_f+0
+        movff   compass_DZ+1, compass_DZ_f+1
+        movff   accel_DX+0,   accel_DX_f+0
+        movff   accel_DX+1,   accel_DX_f+1
+        movff   accel_DY+0,   accel_DY_f+0
+        movff   accel_DY+1,   accel_DY_f+1
+        movff   accel_DZ+0,   accel_DZ_f+0
+        movff   accel_DZ+1,   accel_DZ_f+1
+        return
+
+;-----------------------------------------------------------------------------
+; Q15 fractional numbers: a * b / 2**16 (UNSIGNED)
+;
+; Uses 16x16->16 multiply, for positiv integers, keeping only the most
+; revelant bits.
+;
+; Used to multiply two Q15 numbers, in the range 0..1,
+; represented as 0..32767, that is a / 2**15.
+;
+; (a/2**15) * (b/2**15) = a*b / 2**30 = (a*b/2**16) / 2**14.
+; So to get back a Q15 number, we need a shift-left...
+    global compass_umul
+compass_umul:
+        rcall   compass_mul_16
+
+; The 2x time, by left-shifting inserting the missing bit:
+compass_mul_2:
+        rlcf    compass_r+2,F           ; Missing bit into carry
+        rlcf    compass_r+0,F
+        rlcf    compass_r+1,F
+        movff   compass_r+0,PRODL      ; return value into ProdH:L
+        movff   compass_r+1,PRODH
+        return
+
+; The 16x16-> multiply:
+compass_mul_16:
+        banksel compass_a
+
+        movf    compass_a+1,W           ; Block ah*bh
+        mulwf   compass_b+1
+        movff   PRODL,compass_r+0       ; and copy
+        movff   PRODH,compass_r+1
+
+        movf    compass_a+0,W           ; Block al*bl
+        mulwf   compass_b+0
+        movff   PRODH,compass_r+2       ; Into fraction byte
+
+        movf    compass_a+1,W           ; Block ah*bl
+        mulwf   compass_b+0
+        movf    PRODL,W
+        addwf   compass_r+2,F           ; Fraction part to carry.
+        movf    PRODH,W                 ; and add16
+        addwfc  compass_r+0,F
+        movlw   0
+        addwfc  compass_r+1,F
+
+        movf    compass_a+0,W           ; Block al*bh
+        mulwf   compass_b+1
+        movf    PRODL,W
+        addwf   compass_r+2,F           ; Fraction part to carry.
+        movf    PRODH,W                 ; and add16
+        addwfc  compass_r+0,F
+        movlw   0
+        addwfc  compass_r+1,F
+
+        return
+
+;-----------------------------------------------------------------------------
+; Q15 fractional numbers: a * b / 2**16 (SIGNED)
+
+        global compass_imul
+compass_imul:
+        rcall   compass_mul_16
+
+        btfss   compass_b+1,7
+        bra     compass_mul_3
+
+        movf    compass_a+0,W
+        subwf   compass_r+0,F
+        movf    compass_a+1,W
+        subwfb  compass_r+1,F
+
+compass_mul_3:
+        btfss   compass_a+1,7
+        bra     compass_mul_4
+
+        movf    compass_b+0,W
+        subwf   compass_r+0,F
+        movf    compass_b+1,W
+        subwfb  compass_r+1,F
+
+compass_mul_4:
+        bcf     compass_r+1,6           ; Copy bit 7 to 6, so keep it after 2x
+        btfsc   compass_r+1,7
+        bsf     compass_r+1,6
+        bra     compass_mul_2
+
+        END