; This is a modem for Coast Guard DGPS transmissions.
; It runs on the DSP56002 EVM card with the Leonid monitor.
; The input is a SSB signal, with the carrier at 1KHz.
; The tuning needs to be within 5 or 10Hz.
; USB and LSB will both work.  The output is 4800 baud
; RTCM data stream on the SCI port.  You can plug this directly
; into your GPS receiver.  It works fine with my Garmin 45 and
; GPS II+ receivers, and the Sandy Hook, NJ beacon at 286 KHz. 
;
; The Orange LED lights up to indicate RTCM word sync,
; and blinks if there is a RTCM parity error.
;
; This is hard-coded for the 200BPS input data rate and 4800 baud
; output data rate. You will have to mess with filter bandwidths,
; etc., to make it work for 100BPS transmissions.
;
; Put together by Doug Braun, N1OWU (dougbert@rcn.com). Many
; bits and pieces have been stolen from other EVM programs.
;
; TODO:
; The filter bandwidths are not necessarily optimum.
; The DCD algorithm is poor (but this does not affect the
; data output, only the DCD LED).
; Activate AutoTune-tune threshold detection to allow better reception of
; out-of-tune signals.
;
; Wish List:
; Auto detection and handling of 100 and 200 BPS trnasmissions.
;
;
; $Header: E:/HAM/EVM/RCS/dgps.asm 1.16 1998/11/23 09:11:24 dbraun Exp $
;
; $Log: dgps.asm $
; Revision 1.16  1998/11/23 09:11:24  dbraun
; Added notes.
;
; Revision 1.15  1998/11/23 08:55:02  dbraun
; Added snazzy amplitude-independent phase detection algorithm.
;
; Revision 1.14  1998/11/23 08:15:43  dbraun
; Perfected parity algorithm
;
; Revision 1.13  1998/11/22 19:16:01  dbraun
; Parity algorithm finally works.
;
; Revision 1.12  1998/11/20 13:50:45  dbraun
; More work on RTCM parity
;
; Revision 1.11  1998/11/20 10:03:36  dbraun
; Ongoing work
;
; Revision 1.10  1998/11/17 21:38:10  dbraun
; Scaled Bit Filter coess to avoid possibility of overflow.
;
; Revision 1.9  1998/11/17 18:02:58  dbraun
; Minor tweaks an dcleanup
;
; Revision 1.8  1998/11/16 21:07:09  dbraun
; Fixed a mistake in previous revision.
;
; Revision 1.7  1998/11/16 20:48:02  dbraun
; Removed Rx processing queue, which seems unnecessary for a
; non-packetized (streaming) application like DGPS.
;
; Revision 1.6  1998/11/15 21:38:06  dbraun
; Increased filter BW from +-80 to +-100 Hz and fixed small bug
; inherited from original code.
;
; Revision 1.5  1998/11/15 20:12:59  dbraun
; Added Raw output option
;
; Revision 1.4  1998/11/13 09:36:20  dbraun
; Rearranged memory usage
;
; Revision 1.3  1998/11/13 08:32:11  dbraun
; Got DCD to work reasonably well
;
; Revision 1.2  1998/11/13 03:46:26  dbraun
; Seems to work
;
; Revision 1.1  1998/11/12 22:11:31  dbraun
; Initial revision
;

;Based on:
;FSK modem for 1200 bps (1200Hz and 2200Hz tones)
; Version: E:/HAM/EVM/RCS/fsk1200.asm 1.5 1997/10/23 13:40:14 dbraun Exp 
;(c) 1995 Pawel Jalocha, SP9VRC
;Free license is given for radio-amateur usage _only_.
;This version of 21st July 1995

        opt mu
        nolist
	include 'leonid'
	include 'ioequlc'
	list
        title 'DGPS Modem'

DCDLED  macro mode      ; DCD LED clr/set/chg
	b\mode #13,X:$FFE4
	endm

UpLED   macro mode      ;UP LED
        b\mode #1,X:$FFE4
        endm

DownLED macro mode      ;DOWN LED
        b\mode #2,X:$FFE4
        endm

YellowLED macro mode    ;CAT line (a yellow LED connected)
        b\mode #3,X:$FFE4
        endm

OrangeLED macro mode    ; (an orange LED connected)
        b\mode #14,X:$FFE4
        endm


SampleFreq      equ 9600.0

BufLen  equ     64      ;sample buffer length
BatchLen equ     8      ;processing batch length (must be > 1)
BatchLenLog equ  3      ;BatchLen = 2 ^ BatchLenLog

DCrise     equ  256     ;DC rise time in samples
			;the larger, the slower the DC follows
RMSrise    equ  256     ;RMS rise time in samples
			;the larger, the slower the RMS follows

AGCenable  equ 1        ;automatic adjustment of the input amplification
AGCalert   equ 1        ;indicate too low or too high audio levels with the LEDs

AGChold  equ  512       ;AGC hold time in samples
AGCfall  equ  128       ;AGC fall time in samples/AGC unit (1.5dB) -  default

  if AGCenable
RMSmin   equ 0.050      ;minimum and maximum RMS values for the front-end AGC
  else
RMSmin   equ 0.010      ;without AGC enabled, the AGCmin and max values serve
			;only to determine when to flash the LEDs.
  endif

RMSmax   equ 0.200      ;do not make these closer than by a factor of 2
                        ;the RMSes are indeed MSes that without the square root

BitFreq  equ 2*200.0/SampleFreq        ;200 bits per second
CarFreq  equ 2*1000.0/SampleFreq        ;center freq. = 1000 Hz
CarDev   equ 2*50.0/SampleFreq        ;deviation = +/- 50 Hz

BitLen   equ @cvi(2.0/BitFreq)          ;bit-length in samples

RawOutput equ 0		; If set, we get raw 200 baud demodulator output

OutputBaudRate	equ 4800    ; RTCM output baudrate (if RawOutput is not set)

RxLevelFollow equ 1.0/32    ;Rx Levels and DCD tracking speed (counts per bit)
RxDCDindicate equ 1         ;indicate DCD status with the red LED
RxClockFollow equ 1.0/4096   ;Rx bit-clock recovery speed (counts per sample)
                            ;too much is too good...

SPY equ 0               ;for spying with SPY.EXE (disables the KISS protocol)

LowFreq = (1000.0-100.0)/SampleFreq     ;receive/transmit filters edges (-6dB)
UppFreq = (1000.0+100.0)/SampleFreq
PassFilterLen equ 256                   ;receive/transmit filters length
                                        ;longer length make sharper filters
                                        ;but introduces longer delays

BitFilterLen equ 2*BitLen

HardLimiter equ 0                       ;hard limit the signal after the filter
                                        ;and then filter the signal again
                                        ;before FM demodulator
                                        ;if this is present the InternalAGC
                                        ;makes little sense.

InternalAGC equ 1       ;somehow helps (or substitutes) the CODEC-level AGC.

AutoTune equ 0          ;bit threshold automatic adjustment
			;This seems unnecessary for a properly-tuned DGPS
			;receiver, which should always be tuned within 5Hz
			;or so.

RxGain		equ 3.0   ;Line In (initial, if AGC enabled) gain


;*************************************************************************
;The actual code (internal/external program RAM)

        LOMEM P:$0000
        HIMEM P:$1FFF

        org p:user_code

	andi #%11110011,mr      ;scaling bits = 00

        move #$FFFF,m1           ;r1 to address the coeff. of all filters
                                 ;or any other linear structures

        move #Buffer,r2          ;for us to address the input and output samples
        move #<4-1,n2
	move #<BufLen*4-1,m2

        move #RxBitFilterTap,r4    ;r4 to address the inp/out bit filter taps
        move #<BitFilterLen-1,m4

        move #InpFilterTap,r5    ;r5 to address the inp/out pass filter taps
        if HardLimiter
          move #<InpFilterTap-InpPreFilterTap,n5
	endif
        move #<PassFilterLen-1,m5

        move #Buffer+2,r7        ;r7 for the CODEC's interrupt routine
        move #<BufLen*4-1,m7

        move #BitFreq,x0        ;initialize the bit-sync filter
        jsr <IQ
        move ab,L:<RxSyncFreq
        move ab,L:<RxSyncPhase

	clr a
        opensc

	if !SPY								  
	  bclr #12,X:<<$FFF0    ;disable SCI Tx interrupts		  
	  bclr #11,X:<<$FFF0    ;disable SCI Rx interrupts		  
	  if OutputBaudRate        ;set new baud rate (copied from LEONID)	  
	   jclr #0,X:<<$FFF1,*  ;wait until all data is sent out	  
	   movep #(xtal+2*16*OutputBaudRate)/(2*2*16*OutputBaudRate)-1,X:<<$FFF2
	  endif								  
	endif								  


        ctrlcd  1,r2,BufLen,MIC,RxGain,RxGain,LINEO|HEADP,0.0,0.0
        opencd SampleFreq/1000.0,HPF     ;start taking samples at given rate


	; Do this after calling opencd...
	if RawOutput&&!SPY
	  bclr #12,X:<<$FFF0    ;disable SCI Tx interrupts		  
	  bclr #11,X:<<$FFF0    ;disable SCI Rx interrupts		  
	  bclr #0,X:m_pcc  ; disconnect the SCI from port C
	  bclr #1,X:m_pcc
	  bclr #2,X:m_pcc
	  bset #1,X:m_pcddr  ; make the former TxD pin an output
	endif


BatchLoop
	waitblk r2,BufLen,BatchLen      ;wait till enough samples for one batch
					;the following code should use r2
					;for addressing the samples
					;r7,m7 must not be used: SSI interrupts
					;r3,m3 must not be used: SCI interrupts and LEONID code
				
				;compute DC levels
	clr a  r2,x1            ;clear sums, save r2
	clr b  X:(r2)+,x0
	if BatchLen>1
	.loop #BatchLen-1       ;average samples with the batch
	  add x0,a X:(r2)+n2,x0
	  add x0,b X:(r2)+,x0
	.endl
	endif
	add x0,a X:(r2)+n2,x0
	add x0,b
	move x1,r2              ;restore r2

	if BatchLenLog>0
	.loop #BatchLenLog      ;scale the average
	  asr a
	  asr b
	.endl           ;now: a = left DC, b = right DC
	endif

	rnd a #(1.0-1.0/DCrise*BatchLen),y0           ;further DC filter
	rnd b #1.0/DCrise*BatchLen,y1
	move Y:<DCleft,x0
	mpy x0,y0,a  a,x0
	macr x0,y1,a  Y:<DCright,x0
	mpy x0,y0,b  b,x0
	macr x0,y1,b  a,Y:<DCleft
	move b,Y:<DCright
			;now subtract the DCs from the data
	move a,y0
	move b,y1  X:(r2),a
	move r2,x1      ;save r2
	.loop #BatchLen
	  sub y0,a
	  move a,X:(r2)+
	  move X:(r2),b
	  sub y1,b  X:(r2+n2),a
	  move b,X:(r2)+n2
	.endl
	move x1,r2              ;restore r2

				;sum up the RMSes of both channels
	clr a  r2,x1            ;clear sums, save r2
	clr b  X:(r2)+,x0
	if BatchLen>1
	.loop #BatchLen-1       ;sum up the signal squares
	  mac x0,x0,a X:(r2)+n2,x0
	  mac x0,x0,b X:(r2)+,x0
	.endl
	endif
	mac x0,x0,a X:(r2)+n2,x0
	mac x0,x0,b
	move x1,r2              ;restore r2

	if BatchLenLog>0
	.loop #BatchLenLog      ;divide by BatchLen
	  asr a
	  asr b
	.endl           ;now: a = left RMS, b = right RMS
	endif
	
	rnd a #(1.0-1.0/RMSrise*BatchLen),y0  ; filter these RMSes to get smoother rise/fall
	rnd b #1.0/RMSrise*BatchLen,y1
	move X:<RMSleft,x0
	mpy x0,y0,a  a,x0
	macr x0,y1,a  X:<RMSright,x0
	mpy x0,y0,b  b,x0
	macr x0,y1,b  a,X:<RMSleft
	move b,X:<RMSright

CheckRMSmin                     ;are the RMSes below the minimum required ?
	move #RMSmin,x0
	cmp x0,b               ; check right channel
	jcc <CheckRMSmax
GainUp      
	if AGCenable
				;if so, increase the CODEC's input gain
	  move X:<AGCcount,a      ;decrement the timeout
	  move #>BatchLen,x0
	  sub x0,a  #>AGCfall,x0
	  move a,X:<AGCcount
	  jgt <CheckRMS_OK        ;leave if not yet zero
	  clr a  x0,X:AGCcount
	  move Y:(r2),a1          ;get the CODEC's input control word
	  move #>$0F0F00,x0
	  and x0,a                ;extract the gain
	  cmp x0,a  #>$010100,x0  ;already maximum ?
	  jeq <Low_RMS_Alert          ;if so flash the red LED
	  add x0,a  #>$F0F000,x0  ;if not, increment the gain by 1
	  move a1,x1
	  move Y:(r2),a1          ;and reload all the control words
	  and x0,a  n2,x0         ;in the output buffer
	  or x1,a  #<4,n2         ;make n2=4 for a moment
	  .loop #BufLen
	    move a1,Y:(r2)+n2
	  .endl
	  move x0,n2              ;restore n2
	  move #0.7071,y0         ;increase the RMSes to follow the gain
	  move X:RMSleft,x0       ;increase faster
	  mpyr x0,y0,a  
	  asl a  X:<RMSright,x0
	  mpyr x0,y0,a a,X:<RMSleft
	  asl a
	  move a,X:<RMSright
	else
  	  jmp <Low_RMS_Alert          ;if so flash the LED
	endif

	jmp <CheckRMS_OK

CheckRMSmax                     ;are the RMSes above the given maximum
	move #>AGChold,x0       ;initialize the AGC hold count-down
	move x0,X:<AGCcount
	move #RMSmax,x0
	cmp x0,b                ;compare right RMS
	jcs <CheckRMS_OK
GainDown 
	if AGCenable
                      		  ;if the RMSes are too high
	  clr a                   ;decrease the CODEC's input gain
	  move Y:(r2),a1          ;get the CODEC's input control word
	  move #>$0F0F00,x0
	  and x0,a  #>$010100,x0  ;extract the gain bits
	  sub x0,a  #>$F0F000,x0  ;attempt to decrease the gain
	  jcs <High_RMS_Alert          ;jump if overflow
	  move a1,x1
	  move Y:(r2),a1          ;reload all the input control words
	  and x0,a  n2,x0         ;in the buffer with the new input gain
	  or x1,a  #<4,n2         ;n2=4 for a moment
	  .loop #BufLen
	    move a1,Y:(r2)+n2
	  .endl
	  move x0,n2              ;restore n2
	  move #0.7071,y0         ;decrease the RMSes to follow expected
	  move X:<RMSleft,x0       ;gain reduction faster
	  mpyr x0,y0,a  X:<RMSright,x0
	  mpyr x0,y0,a a,X:<RMSleft
	  move a,X:<RMSright
	endif  ; if AGCenable is 0, fall into High_RMS_Alert
  
High_RMS_Alert
        if AGCalert
          UpLED set
        endif
	jmp <CheckRMSend

Low_RMS_Alert
        if AGCalert
          DownLED set
        endif
	jmp <CheckRMSend

CheckRMS_OK
        if AGCalert
          UpLED clr
          DownLED clr
        endif
CheckRMSend
	

	.loop #BatchLen
;Receiver part
          move X:<RxCarPhase,x0  ;get the receiver carrier phase
	  move X:<RxCarFreq,a    ;advance the phase
	  add x0,a
	  move a1,X:<RxCarPhase
	  jsr <IQ               ;compute I and Q (modifies a,b,x,y,r0,m0,n0)
          move a,x1             ;save cosine
          move b,y1             ;save sine

	  move (r2)+
          move X:(r2),a        ;get input sample from the right channel
	  move (r2)+
	  move (r2)+
	  move (r2)+

          if HardLimiter
          move (r5)-n5          ;r5 = pre-filter tap
          move a,Y:(r5)
          move #PassFilterI,r1 ;apply the pre-filter
          nop
          clr a  X:(r1)+,x0 Y:(r5)+,y0
          .loop #PassFilterLen-1
            mac x0,y0,a  X:(r1)+,x0  Y:(r5)+,y0
          .endl
          mac x0,y0,a  #$7FFFFF,x0
          move #<$80,a                  ;hard limit, a = -1.0
          tpl x0,a                   ;unless a was non-negative - then a = +1.0
          move (r5)+n5
          endif

          move a,Y:(r5)         ;place the sample in the input tap
          move #PassFilterI,r1 ;apply the I-filter
          nop
          clr a  X:(r1)+,x0 Y:(r5)+,y0
          .loop #PassFilterLen-1
            mac x0,y0,a  X:(r1)+,x0  Y:(r5)+,y0
	  .endl
          macr x0,y0,a  #PassFilterQ,r1 ;apply the Q-filter
          nop
          clr b  X:(r1)+,x0 Y:(r5)+,y0
          .loop #PassFilterLen-1
            mac x0,y0,b  X:(r1)+,x0  Y:(r5)+,y0
	  .endl
          macr x0,y0,b a,x0
          move b,y0


;          if HardLimiter
;          tst a  #$7FFFFF,x0
;          move #<$80,a                  ;hard limit, a = -1.0
;          tpl x0,a                      ;unless a was non-negative - then a = +1.0
;          tst b
;          move #<$80,b
;          tpl x0,b
;          move a,x0
;          move b,y0
;          endif

	  ; Mix signal with receiver carrier
          mpy x0,x1,a
          macr y0,y1,a          ;a = demodulated I
          mpy x0,y1,b
          macr -x1,y0,b         ;b = demodulated Q

	  jsr	<RoughNormIQ   ; Normalize to avoid loss of accuracy in Phase
	  jsr	<Phase		; A gets RX phase (similar to arctan(I/Q))

	  ; Note: This phase-detecting algorithm is mostly independent
	  ; of the signal amplitude, unlike the previous approach.

	  ; Calculate the phase difference.
	  move    x:<RxPhase,x0	; Previous sample's phase
	  sub	x0,a	a,x:<RxPhase

   	  move	a1,a		; needs to be able to wrap around without
	  			; worrying about overflow, limiting, etc.

          move #RxBitFilter,r1    ;apply the bit-filter
          move a,Y:(r4)         ;put new data sample into the bit-filter tap
          clr a  X:(r1)+,x0 Y:(r4)+,y0  ;filter the bit data tap
          .loop #BitFilterLen-1
            mac x0,y0,a  X:(r1)+,x0 Y:(r4)+,y0
          .endl
          macr x0,y0,a        ;a=filtered rx data

	  ;in case of overflow, force limiting
	  move	a,a1

          if SPY
           jsr <SpyA
          endif

          move X:<RxDemodOut,x0         ;differenciate to remove mis-tune
          sub x0,a  a,X:<RxDemodOut     ;and save this sample
          rep #3                        ;scale up
            asl a
          move a,x0                     ;square
          mpyr x0,x0,b
          move X:<RxDemodSqr,x0         ;differenciate again to remove
          sub x0,b  b,X:<RxDemodSqr     ;the DC introduced by squaring
          rep #2                        ;scale up again
            asl b

;          if SPY
;          move X:<RxUppLev,a     ;optimal threshold: (lower+upper)/2
;          move X:<RxLowLev,x0
;          add x0,a  X:<RxDemodOut,x1
;          asr a
;          sub x1,a
;          jsr <SpyA
;          endif
                                        ;advance the sync. phase
          move L:<RxSyncPhase,x         ;x1=I, x0=Q
          move L:<RxSyncFreq,y
          mpy x1,y1,a                           ;compute I
          macr -x0,y0,a
          mpy x1,y0,a  a,x1                     ;save I in x1
          macr x0,y1,a  #1.0-RxClockFollow,y0   ;compute Q
          move a,x0                             ;save Q in x0
          mpyr x1,y0,a  #RxClockFollow,y1       ;mult. by decay factor
          mpyr x0,y0,a  a,x1
          move b,x0                             ;add the exciting input to Q
          macr x0,y1,a  Y:<RxSyncPhase,b
          move a,x0
          tst b  x,L:<RxSyncPhase   ;check for non-positive -> positive transition
          jgt <EndBitProcessing
          tst a
          jle <EndBitProcessing         ;jump if not a desired transition
          jset #23,x1,EndBitProcessing  ;jump if I-part negative

            move X:<RxDemodOut,a ; get data
            tst a  #<RxLowLev,r1  ;below or above zero ?
            jlt UpdLevelRMS
            move (r1)+
            move (r1)+
UpdLevelRMS move #1.0-RxLevelFollow,y0   ;update the level
            move #RxLevelFollow,y1
            move X:(r1),x0
            mpy x0,y0,b  a,x0
            macr x0,y1,b
            sub b,a b,X:(r1)+     ;subtract average level
                                  ;compute absolute deviation
            abs a X:(r1),x0       ;update the average deviation
            mpy x0,y0,b  a,x0
            macr x0,y1,b
            move b,X:(r1)

            if AutoTune
	      move X:<RxUppLev,a     ;optimal threshold: (lower+upper)/2
	      move X:<RxLowLev,x0
	      add x0,a 
	      asr a  
	      move X:<RxDemodOut,x0  ;get the sample
	      sub x0,a               ;subtract threshold from data
				     ;bit #23,a1 is the data bit (but beware...)
	      move	a,a1	     ;force limiting
	    else		   
	      move X:<RxDemodOut,a  ;get the sample
            endif

	    move a,X:<RxBit		;save the bit

  ;         if SPY
  ;          jsr <SpyA
  ;         endif


            jsr <procbit
            jsr <CheckRTCMWordSync


EndBitProcessing

          move (r5)-            ;advance the input tap pointer
          move (r4)-            ;advance the bit filter tap pointer

        .endl                   ; #BatchLen

        ;DCD decision based on eye-opening
        move X:<RxLowLev,x0             ;see the difference between 0 and 1 levels
        move X:<RxUppLev,a
        sub x0,a X:<RxLowMS,x1
        abs a X:<RxUppMS,b              ;a = |upp-low|
        add x1,b  a,x1                  ;b = MS(low)+MS(upp)
        if HardLimiter
          asl b
        endif
;       asl b                  ;add more "asl b" if the DCD is too sensitive
;       asl b
        move X:<RxSyncPhase,x0  ;sum-up the energy in the clock-sync filter
        mpy x0,x0,a  Y:<RxSyncPhase,x0
        mac x0,x0,a
        jset #1,X:<HaveCarrier,CheckCarOff
          asl b    #>$000020,x0
          cmp x1,b              ;eye open ?
          jge <CheckCarEnd
   ; N1OWU: The clock signal test needs debugging
   ;      cmp x0,a              ;enough clock signal ?
   ;      jlt <CheckCarEnd
          bset #1,X:<HaveCarrier
          if RxDCDindicate
            DCDLED set
          endif
          jmp <CheckCarEnd
CheckCarOff
        cmp x1,b   #>$000010,x0
        jge <SetCarOff
        cmp x0,a
        jge <CheckCarEnd
SetCarOff
        bclr #1,X:<HaveCarrier
        if RxDCDindicate
          DCDLED clr
        endif
CheckCarEnd

	jsr	ShowDataOnLED


	jmp     <BatchLoop




; Sample the SCI Tx data line and turn on/off the yellow LED
; accordingly, to show when data is being sent out the serial port.
; (This ought to be done in Leonid...)

ShowDataOnLED
	btst	#1,x:$ffe5   ; Port C data register (SCI output)
	.if <cs>
	  YellowLED clr
	.else
	  YellowLED set
	.endi
	rts


procbit

	btst #23,X:<RxBit  ; Carry flag gets received bit

	if RawOutput  ; Send bit directly out TxD line
	  if !SPY
	    .if <cs>
	      bset #1,X:m_pcd
	    .else
	      bclr #1,X:m_pcd
	    .endi
	  endif

	else   ; send as part of a RTCM byte

	  move	X:<RTCMShiftReg,a1   
	  ror	a		    ; shift new bit into MSB of data word
	  move	a1,X:<RTCMShiftReg

	  clr	b
	  move	X:<RTCMShiftCount,b0  
	  dec b
	  move	b0,X:<RTCMShiftCount  
	  .if <le>
	    ; bit count has reached zero, so reset the counter
	    ; and send out a data byte
	    move	#>6,x0
	    move	x0,X:<RTCMShiftCount   

	    move	X:<RTCMShiftReg,a1   
	    ; shift MS 6 bits to LS 6 bits
	    rep #18
	      lsr a

	    bset #6,a1   ;correctly set two highest bits as per RTCM spec
	    bclr #7,a1
	    move	a1,x0
	    if !SPY
	      putc
	    endif

	  .endi
	endif

	rts


MaxScore equ 5

CheckRTCMWordSync

	; Check the word sync
	move	L:<WordShiftReg,a   
	asl	a		; shift new bit into LSB of data word
	btst #23,X:<RxBit  	; Carry flag gets received bit
	.if <cs>
	  bset #0,a0
	.endi
	bclr	#8,a1		;Keep bits 32-47 clear
	move	a1,X:<WordShiftReg     ; Avoid data limiting
	move	a0,Y:<WordShiftReg

	clr	b
	move	X:<WordShiftCount,b0  
	dec b
	move	b0,X:<WordShiftCount  
	.if <le>
	  ; bit count has reached zero, so reset the counter
	  move	#>30,x0
	  move	x0,X:<WordShiftCount   

	  ; and check the word's parity
	  move	L:<WordShiftReg,a   
	  jsr	CheckRTCMWordParity   ; Sets Z flag
	  .if <eq>
	    ; Good parity!
	    inc		b
	    clr		a
	    move	#>MaxScore,a0
	    cmp		a,b
	    .if	<ge>
	      move	#>MaxScore,b0
	      bset	#0,X:<InSync
	      OrangeLED set
	    .endi
	  .else
	    ; Bad parity :-(
	    dec		b
	    .if	<le>
	      clr	b
	      bclr	#0,X:<InSync
	      ; Force a re-search of the word sync
	      move	#>1,x0
	      move	x0,X:<WordShiftCount   
	    .endi
	    OrangeLED clr
	  .endi

	  move	b0,X:<WordSyncScore  

	.endi

	rts



; This checks the 32-bit word in A, and returns Z=1 if the
; RTCM parity is correct.  The RTCM spec itself is not freely
; available (you have to send $$$ to RTCM), but the parity
; algorithm is the same as that of the GPS signal itself,
; which is specified in:
;   http://www.navcen.uscg.mil/gps/geninfo/gpsdocuments/sigspec/default.htm
;
; In a nutshell, a 30-bit RTCM word has six parity bits (the LS six bits).
; The algorithm also uses the two LS parity bits of the previous word,
; which are the two MS bits of the current 32-bit word.
;
CheckRTCMWordParity

	; First, see if the word is an illegal value
	; of 0 or ffffffff
	clr	b
	cmp	b,a
	.if <eq>
	  andi	#$fb,ccr	;clear zero flag
	  rts
	.endi

	move	L:<RTCM32BitMask,b
	cmp	b,a
	.if <eq>
	  andi	#$fb,ccr	;clear zero flag
	  rts
	.endi

	; Second, complement the data bits if the previous
	; word's LS parity bit is set.
	btst	#6,a1
	.if <cs>
	  move	L:<RTCMDataMask,x
	  move	a0,b1
	  eor	x0,b1
	  move	b1,a0
	  eor	x1,a1
	.endi
	move	a1,X:<CurWord	; Evade possible limiting
	move	a0,Y:<CurWord

	clr	b
	move	#RTCMParityMasks,r1
	.loop #6
	  asl	b	; shift the previous parity bit
	  move	L:<CurWord,a  ; Get the data word
	  move	L:(r1)+,x  ; Get current parity mask
	  move	a0,b1      ; AND it with the data word
	  and		x0,b1
	  move	b1,a0
	  and		x1,a1
	  jsr	ParityA		   ;Get the parity into LSB of b0
	.endl

	; Now do an XOR of the transmitted and actual parity bits
	move	L:<CurWord,a  ; Get the data word
	move	a0,a1	      ;Get the least significant part in a1
	move	b0,x1	
	eor	x1,a1         ;xor the calculated parity bits with the
			    ;transmitted ones
	clr	b
	move	X:<WordSyncScore,b0

	move	#>$3f,x1      ;mask the parity error bits
	and	x1,a1	      ;Z flag is set

	rts


; This checks the simple parity of the 32 LS bits of A, and returns it
; in the LSB of B0.  A is modified. The other bits of B0 are not changed.

ParityA
	bclr #0,b0
	.loop #32
	  asr	a
	  .if <cs>
	    bchg #0,b0
	  .endi
	  nop  ;Necessary to avoid nasty illegal situation of jump to .endl
	.endl
	rts


PI      equ     3.14159265358979323846
EX      equ     2.718281745911

;this routine computes a cosine/sine pair using the sine ROM
;with a second order (linear+quadrature) approximation between table points
IQ                              ;x0 = angle ( -1 = -PI, +1 = +PI)
        move #>$80,x1   ;shift out 8 most significant bits
        mpy x0,x1,a  #>$FF,x0
        ori #%00000011,mr       ;disable interrupts
        move x0,m0
        and x0,a     #>$100,x0
        or x0,a      #<$40,n0
        ori #%00000100,omr      ;enable the sine ROM table
        move a1,r0      ;put the 8 most significant bits into r0 with offset = $100
        move a0,y0      ;save the remaining bits in y0
        jclr #23,y0,SinTable
          move (r0)+
SinTable
        move Y:(r0+n0),x0       ;x0 = coarse cosine
        move Y:(r0),x1          ;x1 = coarse sine
        mpyr x1,y0,a  #PI/256.0,y1
        tfr x0,a  a,x1
        macr -x1,y1,a           ;a = fine cosine
        mpyr x0,y0,b  Y:(r0),x1
        andi #%11111011,omr     ;disable the sine ROM table
        tfr x1,b  b,x1
        macr x1,y1,b  #PI*PI/2.0/65536.0,y1  ;b = fine sine
        mpyr y0,y0,a  a,x0
        andi #%11111100,mr      ;enable interrupts
        move a,y0
        mpyr y0,y1,a
        tfr x0,a  a,y1
        macr -x0,y1,a  b,x1     ;a = super fine cosine
        macr -x1,y1,b           ;b = super fine sine
        rts                     ;x,y are modified
                                ;r0,m0,n0 are modified
                                ;maximum error is about 0.7E-6
                                ;execution time 4+64+4 clock cycles
                                ;including "jsr <IQ" and "rts"
;enable/disable interrupts must be there if interrupt routines
;are accesing RAM at Y:$100..$1FF (like CODEC input/output buffers)



;This routine scales an I/Q pair (in AB) such that the larger number
;is within the range +/-0.5..1.0

RoughNormIQ             ;scale an I/Q pair such that the larger number
                        ;is within the range +/-0.5..1.0
        cmpm a,b        ;which number is greater ?
        jge <NormAftB   ;if b then follow the normalization according to it
        tst a           ;otherwise follow a
        jes <RightA     ;if too big (extension in use) the shift to the right
        jnr <DoneA      ;if normalized then we are done
LeftA     asl b         ;otherwise shift left both numbers
	  asl a
        jnn <LeftA      ;until A is normalized
        rts
RightA    asr b         ;if extension set then shift both number to the right
	  asr a
        jes <RightA     ;until a is normalized
DoneA   rts

NormAftB                ;same stuff for b
	tst b
	jes <RightB
        jnr <DoneB
LeftB	  asl a
	  asl b
	jnn <LeftB
        rts
RightB	  asr a
	  asr b
	jes <RightB
DoneB   rts


;The following routine computes the phase of the given I/Q pair
;this routine is not very precise: maximum error is 2E-4 (2E-4/PI radians)
;the error could be made lower but I required that the calculation
;is exact for PI/4, PI/2, PI*3/4, etc.
Phase      ;a = I-part (24 bits), b = Q-part (24 bits)
           ;-1.0 <= a < 1.0, -1.0 <= b < 1.0
        tst b #<0,x0    ;if Q-part negative => turn by PI
        .if <mi>
          neg a #<%10000000,x0
          neg b
        .endi
        tst a           ;if I-part negative => turn by -PI/2
        .if <mi>
          bset #22,x0
          tfr a,b b,a   ;swap I with Q
          neg b         ;negate the new Q
        .endi
        add b,a                 ;add Q to I (what if I+Q >= 1.0 ?)
        asr a                   ;I+Q /= 2 in case it is > 1
        asr b a,x1              ;Q /= 2 because of the above, x1 = I+Q
        andi #$FE,ccr           ;divive Q by (I+Q)
        rep #24
          div x1,b
        tfr x0,a b0,x0          ;x0 = Q div (I+Q), a=approx angle
        tfr x0,b #0.5,x0        ;b = Q/(I+Q)
        sub x0,b #0.25,x0       ;b = Q/(I+Q) - 1/2 =: X
        add x0,a b,x1 #0.63361654,y1 ;angle += 0.25, x1 = X
        mac x1,y1,a x1,x0       ;angle += F1*X, x0 = X
        mpyr x0,x0,b #-0.73728217,y1 ;b = X*X
        mpyr x1,y1,b b,x0       ;b = F3*X, x0 = X*X
        move b,y1
        mac x0,y1,a x0,y0       ;angle += F3*X * X*X, y0 = X*X
        mpyr x0,x0,b #0.85568791,y1 ;b = (X*X)*(X*X)
        mpyr x1,y1,b b,x0       ;b = F5*X, x0 = X*X*X*X
        move b,y1
        mac x0,y1,a             ;angle += F5*X * X*X*X*X
        mpyr x0,y0,b #-0.17769569,y1 ;b = (X*X*X*X)*(X*X)
        mpyr x1,y1,b b,x0       ;b = F5*X, x0 = X*X*X*X*X*X
        move b,y0
        macr x0,y0,a            ;angle += F5*X * X*X*X*X*X*X
        rts             ;a = the phase (-1.0 => -PI, 1.0 => PI)
                        ;b,x,y are modified
                        ;execution time is about 130 cycles (65 instructions)



        if SPY

SpySync move a10,L:<SpySave     ;output: carry=1 => no spy request
        move a2,X:<SpySave+1    ;carry=0 => spy request !
        move x0,Y:<SpySave+1    ;512 words (jsr <SpyA) should follow
        move x1,Y:<SpyCount
        move X:<SpyCount,a
        tst a
        jne <SpyCont
        lookc 0
        jcs <Spy_end
        move #>'S',a
        cmp x0,a
        ori #$01,ccr
        jne <Spy_end
        move #>'P',x0
        putc
        move #>0,x0      ; send a 0 meaning non-complex data
        putc
        move #>512,a
        move a,X:<SpyCount
SpyCont andi #$FE,ccr
        jmp <Spy_end

SpyA    move a10,L:<SpySave
        move a2,X:<SpySave+1
        move x0,Y:<SpySave+1
        move x1,Y:<SpyCount
        move X:<SpyCount,a
        tst a
        jne <Spy_copy

Spy_check
        lookc 0
        jcs <Spy_end
        move #>'S',a
        cmp x0,a
        jne <Spy_end
        move #>'P',x0
        putc
        move #>0,x0      ; send a 0 meaning non-complex data
        putc
        move #>512,a
Spy_copy
        move #>1,x0
        sub x0,a
        move a,X:<SpyCount

        move X:<SpySave,a
        rep #8
          lsr a
        move a1,x0
	putc
        move X:<SpySave,a
        rep #16
          lsr a
        move a1,x0
        putc

Spy_end move L:<SpySave,a10
        move X:<SpySave+1,a2
        move Y:<SpySave+1,x0
        move Y:<SpyCount,x1
        rts

        endif



LastP = *

;*************************************************************************
;Internal data RAM

        LOMEM X:$0000,Y:$0000,L:$0000
        HIMEM X:$00FF,Y:$00FF,L:$00FF

        org L:user_data

        if SPY
SpySave dc 0,0
SpyCount dc 0
        endif

RxSyncFreq dc 0
RxSyncPhase dc 0

WordShiftReg   dc 0  ; A 32-bit value
CurWord   dc 0  ; A 32-bit value

; Magic numbers for RTCM parity algorithm
RTCMDataMask	dc	$3fffffc0
RTCM32BitMask	dc	$ffffffff
RTCMParityMasks
		dc	$bb1f3480
		dc	$5d8f9a40
		dc	$aec7cd00
		dc	$5763e680
		dc	$6bb1f340
		dc	$8b7a89c0

LastL = *

        org X:LastL
        org Y:LastL

        org Y:
DCleft  dc 0            ;DC bias for both channels
DCright dc 0
	
        org X:
RMSleft  dc 0           ;RMS (energy) for both channels
RMSright dc 0

	org X:
AGCcount dc 0           ;counter for the AGC hold-off

        org X:
RxCarFreq   dc CarFreq
RxCarPhase  dc 0

RxI dc 0        ;most recent I-Q vector
RxQ dc 0
RxAmpl dc 0     ;amplitude (mean square) of the FM demod. output
RxPhase dc 0     ;Rx phase

RxDemodOut  dc 0
RxDemodSqr  dc 0

RxLowLev    dc 0        ;keeps levels and deviations for the received symbols
RxLowMS     dc 0
RxUppLev    dc 0
RxUppMS     dc 0

RxBit		dc 0	;Boolean value in bit #23
HaveCarrier   dc 0      ;Boolean value in bit #0
InSync   dc 0      ;Boolean value in bit #0
WordSyncScore	dc 0   ;Integer

RTCMShiftReg   dc 0
RTCMShiftCount  dc 6

WordShiftCount  dc 32

;PI      equ     3.141592654


RxBitFilter

; This filter expects to be two bits long
sigma = @cvf(BitLen)/6.0

time = -@cvf(BitFilterLen/2)+0.5
count   set 0
        dup BitFilterLen
Filter = @pow(EX,-time*time/(2.0*sigma*sigma))
time = time+1.0
        dc Filter/BitLen	; Scale coeffs to avoid overflow
count   set count+1
        endm

;*************************************************************************
;External data RAM

      LOMEM X:$0100,Y:$0100,L:$0100
      HIMEM X:$3FFF,Y:$3FFF,L:$3FFF

; DB: Put the external X data right after the P (program) data.

        org X:@cvs(X,LastP)

PassFilterI

sigma = @cvf(PassFilterLen)/2.0/3.0

	org	X:PassFilterI

time = -@cvf(PassFilterLen/2)+0.5
count   set 0
        dup PassFilterLen
Window = @pow(EX,-time*time/(2.0*sigma*sigma))
Filter = (@sin(2.0*PI*time*UppFreq)-@sin(2.0*PI*time*LowFreq))/(PI*time)
time = time+1.0
        dc  Window*Filter
count   set count+1
        endm


PassFilterQ

time = -@cvf(PassFilterLen/2)+0.5
count   set 0
        dup PassFilterLen
Window = @pow(EX,-time*time/(2.0*sigma*sigma))
Filter = (-@cos(2.0*PI*time*UppFreq)+@cos(2.0*PI*time*LowFreq))/(PI*time)
time = time+1.0
        dc  Window*Filter
count   set count+1
        endm


LastX = *


; Uninitialized buffers in external Y memory

        org Y:$0100

        if HardLimiter
InpPreFilterTap  dsm PassFilterLen
        endif
InpFilterTap  dsm PassFilterLen

RxBitFilterTap dsm BitFilterLen


LastY = *


        if @cvi(LastX)>=@cvi(LastY)
          org L:LastX
        else
          org L:LastY
        endif

	; Make sure this is above last P/X/Y memory location

Buffer   dsm BufLen*4   ;CODEC's input/output buffer
LastL = *

	end

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