; Implantable Inertial Sensor Program P3035A09.asm

; ------------------------------------------------------------

; Constant Declarations

; Address Map Constants
const mmu_shb  0x1E00 ; Sensor Data HI Byte 
const mmu_slb  0x1E01 ; Sensor Data LO Byte
const mmu_sar  0x1E02 ; Sensor Address Register
const mmu_scr  0x1E04 ; Sensor Control Register
const mmu_irqb 0x1E10 ; Interrupt Request Bits
const mmu_imsk 0x1E12 ; Interrupt Mask Bits
const mmu_irst 0x1E14 ; Interrupt Reset Bits
const mmu_iset 0x1E16 ; Interrupt Set Bits
const mmu_itp  0x1E18 ; Interrupt Timer Period 
const mmu_rst  0x1E19 ; System Reset
const mmu_xhb  0x1E20 ; Transmit HI Byte
const mmu_xlb  0x1E21 ; Transmit LO Byte
const mmu_xcn  0x1E22 ; Transmit Channel Number
const mmu_xcr  0x1E24 ; Transmit Control Register
const mmu_xfc  0x1E26 ; Transmit Frequency Calibration
const mmu_etc  0x1E30 ; Enable Transmit Clock
const mmu_tcf  0x1E32 ; Transmit Clock Frequency
const mmu_tcd  0x1E34 ; Transmit Clock Divider
const mmu_bcc  0x1E36 ; Boost CPU Clock
const mmu_tpr  0x1E38 ; Test Point Register
const mmu_mia  0x1E3A ; Multiplier Input A
const mmu_mib  0x1E3B ; Multiplier Input B
const mmu_moh  0x1E3C ; Multiplier Output HI	
const mmu_mol  0x1E3D ; Multiplier Output LO

; Sensor Control Constants
const gy_rd16    0x05 ; Read Sixteen-Bit Word from Gyroscope
const gy_wr16    0x07 ; Write Sixteen-Bit Word to Gyroscope
const gy_rd8     0x01 ; Read Eight-Bit Byte from Gyroscope
const gy_wr8     0x03 ; Write Eight-Bit Byte to Gyroscope
const acc_rd16   0x04 ; Read Sixteen-Bit Word from Accelerometer
const acc_wr16   0x06 ; Write Sixteen-Bit Word to Accelerometer
const acc_rd8    0x00 ; Read Eight-Bit Byte from Accelerometer
const acc_wr8    0x02 ; Write Eight-Bit Byte to Gyroscope

; Sensor Register Locations
const sens_tmr_0          0x17
const sens_tmr_1          0x18
const sens_tmr_2          0x19
const acc_pwr_conf        0x7C
const acc_features_in     0x5E
const acc_init_ctrl       0x59
const acc_internal_status 0x2A

; Configuration Constants
const sp_initial    0x1700 ; Bottom of the stack in RAM.
const tk_divisor    12     ; Number of ring periods per 100 ns.
const acc_init_cntr 0x0C00 ; Value for HL to count down from.
const acc_init_dly  15     ; Number of RCK periods for 450 us.
const min_tcf       75     ; Minimum TCK periods per half RCK period.
const tx_frequency  6      ; Transmit frequency calibration

; Variable locations
const counter_var   0x0000  ; A generic eight-bit counter

; ------------------------------------------------------------

; The CPU reserves two locations 0x0000 for the start of program
; execution, and 0x0003 for interrupt execution. We put jumps at
; both locations. A jump takes exactly three bytes.
start: 
jp initialize
jp interrupt

; ------------------------------------------------------------

; Make a HI pulse on tp_reg(0).
high_pulse:
push A           ; Push A onto the stack to save it
ld A,0x01        ; Load A with bit zero set to one.
ld (mmu_tpr),A   ; Set the test point one bit.
ld A,0x00        ; Load A with all bits zero.
ld (mmu_tpr),A   ; Clear the test point one bit.
pop A            ; Restore A
ret              ; Return from subroutine.

; ------------------------------------------------------------

; Transmits gyroscope sensor timer on channel stored in A.
xmit_gy_time:
push A           ; Push A onto the stack to save it
ld (mmu_xcn),A   ; Write the channel offset
ld A,sens_tmr_1  ; Load A with timer byte one address
ld (mmu_sar),A   ; Write address to sensor address register
ld A,gy_rd16     ; Load A with gyroscope sixteen-bit read command
ld (mmu_scr),A   ; Send command to sensor controller
ld A,(mmu_shb)   ; Read sensor HI byte into A
ld (mmu_xhb),A   ; Write sensor HI byte to transmit HI byte
ld A,(mmu_slb)   ; Read sensor LO byte into A
ld (mmu_xlb),A   ; Write sensor LO byte to transmit LO byte
ld (mmu_xcr),A   ; Any write to transmit control register
pop A            ; Restore A
ret              ; Return from subroutine.

; ------------------------------------------------------------

; Transmits SP on channel stored in A
xmit_SP:
push A           ; Push A onto the stack to save it
push H           ; Push H onto the stack to save it
push L           ; Push L onto the stack to save it
ld (mmu_xcn),A   ; Write the channel offset
LD HL,SP         ; Load HL with the stack pointer
push H           ; Push H onto the stack
pop A            ; Pop H into A
ld (mmu_xhb),A   ; Write H to transmit HI byte
push L           ; Push L onto the stack
pop A            ; Pop L into A
ld (mmu_xlb),A   ; Write L to transmit LO byte
ld (mmu_xcr),A   ; Any write to transmit control register
pop L            ; Restore L
pop H            ; Restore H
pop A            ; Restore A
ret              ; Return from subroutine.

; ------------------------------------------------------------

; Transmits BC on channel stored in A
xmit_HL:
push A           ; Push A onto the stack to save it
ld (mmu_xcn),A   ; Write the channel offset
push H           ; Push H onto the stack
pop A            ; Pop H into A
ld (mmu_xhb),A   ; Write H to transmit HI byte
push L           ; Push L onto the stack
pop A            ; Pop L into A
ld (mmu_xlb),A   ; Write L to transmit LO byte
ld (mmu_xcr),A   ; Any write to transmit control register
pop A            ; Restore A
ret              ; Return from subroutine.

; ------------------------------------------------------------

; Replace BC with product of B and C.
product_BxC:
push A           ; 1 Push A onto the stack to save it
push B           ; 1 Push X onto the stack
pop A            ; 2 Pop X off the stack into A
ld (mmu_mia),A   ; 3 Write X to multiplier input A
push C           ; 1 Push Y onto the stack
pop A            ; 2 Pop Y off the stack
ld (mmu_mib),A   ; 3 Write Y to multiplier input B
ld A,(mmu_moh)   ; 4 Read HI byte of product XY
push A           ; 1 Push HI byte onto the stack
pop B            ; 2 Pop HI byte into B
ld A,(mmu_mol)   ; 4 Read LO byte of product XY
push A           ; 1 Push LO byte onto the stack
pop C            ; 2 Pop LO byte into C
pop A            ; 2 Restore the original value of A
ret              ; 4 Return from subroutine

; ------------------------------------------------------------

; Increment value pointed to by IX and transmit its square on the 
; channel given by A.
tx_IX2_inc:
push A           ; Push A onto the stack to save it
push B           ; Push B onto the stack to save it
push C           ; Push C onto the stack to save it
push D           ; Push D onto the stack to save it
push E           ; Push E onto the stack to save it
push H           ; Push H onto the stack to save it
push L           ; Push L onto the stack to save it
push A           ; Copy the transmit channel number
pop E            ; and save it in E
ld A,(IX)        ; Load the number we are to increment
inc A            ; increment
ld (IX),A        ; and save to memory.
push A           ; Copy the number into
pop B            ; register B
push A           ; and also 
pop C            ; register C
call product_BxC ; Obtain B x C
push B           ; Copy B
pop H            ; to H
push C           ; Copy C
pop L            ; to L
push E           ; Copy the transmit channel number
pop A            ; into A
call xmit_HL
pop H            ; Restore H
pop L            ; Restore L
pop E            ; Restore E
pop D            ; Restore D
pop C            ; Restore C
pop B            ; Restore B
pop A            ; Restore A
ret              ; Return from subroutine

; ------------------------------------------------------------

; Calibrate the transmit clock frequency. Will leave the
; transmit clock disabled and cpu boost turned off.
calibrate_tck:
push A           ; Push A onto the stack to save it
push B           ; Push B onto the stack to save it
ld A,0x00        ; Clear bit zero of A
ld (mmu_bcc),A   ; Disable CPU Clock Boost
ld (mmu_etc),A   ; Disable Transmit Clock
ld A,15          ; The initial value of divisor
push A           ; Push divisor onto the stack
pop B            ; Store divisor in B
cal_tck_1:
dec B            ; Decrement the divisor
push B           ; Push divisor onto stack
pop A            ; Pop divisor into A
ld (mmu_tcd),A   ; Write divisor value to ring oscillator
ld A,0x01        ; Set bit zero of A
ld (mmu_etc),A   ; Enable the transmit clock
ld A,(mmu_tcf)   ; Read the transmit clock frequency
sub A,min_tcf    ; Subtract the minimum frequency
ld A,0x00        ; Clear bit zero of A
ld (mmu_etc),A   ; Disable Transmit Clock
jp np,cal_tck_1  ; Try smaller divisor
pop B            ; Restore B
pop A            ; Restore A
ret              ; Return from subroutine

; ------------------------------------------------------------

; Here we initialize the accelerometer without using the stack,
; because the entire RAM, including stack space, is occupied 
; by the 6-Kbytes of accelerometer configuration data. At the
; end, we jump back to the main initialization routine to an
; label we assume exists: done_init_accelerometer.
init_acceleromater:

; Disable accelerometer advanced power save mode.
ld A,acc_pwr_conf   
ld (mmu_sar),A
ld A,0x00
ld (mmu_slb),A
ld A,acc_wr8
ld (mmu_scr),A

; Wait for 450 us for accelerometer, which we do with RCK periods.
ld A,acc_init_dly
dly A

; Write to the accelerometer initialization control register.
ld A,0x00
ld (mmu_slb),A
ld A,acc_init_ctrl
ld (mmu_sar),A
ld A,acc_wr8
ld (mmu_scr),A

; Write accelerometer feature initialization file, which is currently
; occupying the entire 6-KByte RAM of the CPU. We cannot use the stack
; yet, because the stack contains initialization data. We load HL with
; a constant and count it down using the Z flag. We want to perform
; 12 sets of 256 sixteen-bit writes to the accelerometer, so we set
; H to 11 and L to 0, and start decrementing.
ld IX,0x0000 
ld HL,acc_init_cntr
ld A,acc_features_in
ld (mmu_sar),A
acc_init_loop:
ld A,(IX)
inc IX
ld (mmu_slb),A  ; Configuration data is LO byte first.
ld A,(IX)
inc IX
ld (mmu_shb),A  ; And HI byte second.
ld A,acc_wr16
ld (mmu_scr),A  ; A sixteen-bit serial write.
dec L
jp nz,acc_init_loop ; If L=0, done with a 512-byte block
dec H
jp nz,acc_init_loop ; If H=0, done with 12 blocks.

; Enable sensor features.
ld A,0x01
ld (mmu_slb),A
ld A,acc_init_ctrl
ld (mmu_sar),A
ld A,acc_wr8
ld (mmu_scr),A

; Wait for sensor, marking iterations by writing to the test
; point register. We will stay in this loop forever if the
; accelerometer does not start up without an error.
ld A,acc_internal_status
ld (mmu_sar),A
acc_wait:
ld A,0x01      
ld (mmu_tpr),A  ; Make a HI pulse on tp_reg(0).
ld A,0x00
ld (mmu_tpr),A  ; Back to LO.
ld A,acc_rd8
ld (mmu_scr),A
ld A,(mmu_slb)  ; Get the status register
sub A,0x01
jp nz,acc_wait  ; Anything other than a 0x01 keeps us in the loop

; Return to main initialization program.
jp done_init_accelerometer

; ------------------------------------------------------------

; Interrupt routine, clears timer interrupt and generates
; a pulse on test point register bit zero.
interrupt:
push F              ; Save the Z, S, C, and I flags onto the stack.
push A              ; Save A on stack
ld A,0x01           ; Set bit zero of A
ld (mmu_irst),A     ; Reset the timer interrupt
call high_pulse     
pop A               ; Restore A
pop F               ; Restore the flags.
rti

; ------------------------------------------------------------

; Initialize the device. Don't use the stack until after the
; accelerometer has been configured.
initialize:

; Wait for a while. The power supplies must settle after entering
; standby mode, and the ring oscillator frequency is sensitive to
; the power supply voltage.
ld A,0xFF
dly A

; Initialize the accelermometer. We jump to the initialization routine, and
; the routine jumps back.
; jp init_accelerometer
done_init_accelerometer:

; Initialize the stack pointer.
ld HL,sp_initial  
ld SP,HL

; Calibrate the transmit clock.
call calibrate_tck

; Set the low frequency for sample transmission
ld A,tx_frequency
ld (mmu_xfc),A

; Initialize counter variable
ld IX,counter_var
ld A,0
ld (IX),A

; Set interrupt timer interval.
ld A,0xFF
ld (mmu_irst),A
ld A,255
ld (mmu_itp),A
ld A,0x01
ld (mmu_imsk),A

; ------------------------------------------------------------

; The main program. We loop through our jobs as fast as we
; can. The interrupt will be running in the background.
main:

ld A,21             ; Select transmit channel
call xmit_gy_time
ld A,49             ; Select transmit channel
call xmit_SP
ld A,36             ; Select transmit channel
call tx_IX2_inc

jp main

; ------------------------------------------------------------
