; ==== ISUAL Flight Software -- Univ of California, Berkeley, SSL ====
; eeprom.a -- "extension" of flight software
;  functions stored in an EEPROM bank

;  (binary is called "FSW_Xnn.B")

; Includes:
;   Clock time conversion routines
;   Power Service on/off
;   CCSDS telemetry packet builder
;   Heater control task
;   Extra ground commands

; Revision History:
;  18-Dec-2002 - SPG - ISUAL Flight Software rev 7.3
;    First Official Release

;  21-Dec-2002 - SPG - FSW 7.5
;   In heater task, changed Front Lens Heater
;   from power service 7 to 8

;  28-Dec-2002 - SPG - FSW F.F
;   add redirect for packet 720
;   Moved I_NOP to EXECUTE.a

;  15-Jan-2003
;    Added jump table entry for "call task"
;    Moved in all I_WAIT_ and I_VERIFY_ commands

; 26-Mar-2003
;    mask out TBD and "raw" bits in DAY_SENS_STS_I - don't enable interrupts

; 2-May-2003
;    fix I_WAIT_MON

; 29-May-2003
;    Separate SOH and science packet generation
;    Eliminate VERIFY commands

; 6-Aug-2003
;    added q_set_rosizes

; 8-Aug-2003
;    moved I_GENERIC_CMD and I_TLM_FLUSH from execute.a

; 9-Sep-2003
;    moved I_CAM_HV_ON, I_SPA_HV_ON, I_SPB_HV_ON
;    from execute.a

; 19-Sep-2003
;    remove duplicate I_DSENS_FORCE_ON/OFF
 
; EEPROM code runs in banked 16K area 
        org 4000h

; (this is not a table - just a handy list of addresses
;  to compare with -.i files)
        dw C_10_to_6
        dw C_6_to_10
        dw Q_CCSDS_HSKP
        dw Q_CCSDS_SCIHDR
        dw Q_CCSDS_SCIDATA
        dw Q_GET_SOHSCI
        dw Q_HTR_INIT
        dw Q_HTR_SET
        dw Q_EXEC2
        dw Pwr_set_on
        dw pwr_set_off
        dw q_set_rosizes

        include GBL.I
        include ioports.i

p10     equ GBL_CLOCKWORK          ; ds 2
p6      equ GBL_CLOCKWORK+2        ; ds 2
year_seconds equ GBL_CLOCKWORK+4   ; ds 2
unit_seconds equ GBL_CLOCKWORK+6   ; db 0,0,0,0
temp6   equ GBL_CLOCKWORK+10        ; ds 6
  ; GBL.I allocates 16 bytes to GBL_CLOCKWORK 

; Copy (C) bytes from [HL] to [DE] 
COPY_BYTES:
cpyl:   mov a,M
        stax d
        inx h
        inx d
        dcr c
        jnz cpyl
; return DE and HL altered; C=0
        ret

; 16-bit subtract
; SUBTRACT [HL] = [HL] - [DE]
SUB16   push psw
        MOV     A,L
        SUB     E
        MOV     L,A
        MOV     A,H
        SBB     D
        MOV     H,A
        pop psw
        RET

; wait at least 1 millisecond
WAIT_1MS: 
        push psw
        mvi a,70        ; <seems to be right>
        out WATCHDOG_PET_O
WL1:    dcr a
        jnz wl1
        mvi a,70
WL2:    dcr a
        jnz wl2
        pop psw
        ret

C_10_to_6:
; HL - points to 10-byte time
; DE - points to 6-byte result

; save pointers
        shld p10 
        xchg
        shld p6

; clear result to zero seconds
        lxi h,temp6
        mvi a,0
        mvi c,4
clr1:   mov M,a
        inx h
        dcr c
        jnz clr1

; get year minus 2000
        lhld p10        
        mov d,M         ; high byte of year
        inx h
        mov e,M         ; low byte of year
        xchg
        lxi d,-2000
        dad d           ; year - 2000
        mov a,l         ; (only care about low byte)
        
; for each year past 2000, add number of seconds in a year
        cpi 0
        jz ny
yl1:    lxi h,seconds_in_a_year+3
        mov c,a         ; save A
        ani 3           ; year is multiple of 4?
        cpi 1           ; (previous year)
        jnz notleapyr
        lxi h,seconds_in_a_leapyear+3
notleapyr: mov a,c      ; restore A
        call add4       ; (adds to 'temp6')
        dcr a           ; count off a year past 2000
        jnz yl1
ny:

; get day number minus 1 (Jan 1 is UT 'day 1')
        lhld p10
        inx h
        inx h           ; point to high byte of day
        mov d,M
        inx h
        mov e,M         ; low byte of day
        dcx d           ; subtract 1

; look at high order byte
; if 1, add number of seconds in 256 days
        mov a,d
        cpi 1
        jnz nd256
        lxi h,seconds_in_256_days+3
        call add4
nd256:

; look at low order byte of day number.
; look at each bit.  add number of seconds
; in 128, 64, 32, 16, 8, 4, 2 and 1 day
        mov a,e
        ani 80h
        jz n80
        lxi h,seconds_in_128_days+3
        call add4
n80:    mov a,e
        ani 40h
        jz n40
        lxi h,seconds_in_64_days+3
        call add4
n40:    mov a,e
        ani 20h
        jz n20
        lxi h,seconds_in_32_days+3
        call add4
n20:    mov a,e
        ani 10h
        jz n10
        lxi h,seconds_in_16_days+3
        call add4
n10:    mov a,e
        ani 08h
        jz n08
        lxi h,seconds_in_8_days+3
        call add4
n08:    mov a,e
        ani 04h
        jz n04
        lxi h,seconds_in_4_days+3
        call add4
n04:    mov a,e
        ani 02h
        jz n02
        lxi h,seconds_in_2_days+3
        call add4
n02:    mov a,e
        ani 01h
        jz n01
        lxi h,seconds_in_1_day+3
        call add4
n01:

; look at hour
; look at each bit, add number of seconds
        lhld p10
        lxi d,4
        dad d           ; point to hour
        mov e,M
        mov a,e
        ani 10h         ; 16
        jz nh10
        lxi h,seconds_in_16_hrs+3
        call add4
nh10:   mov a,e
        ani 08h
        jz nh08
        lxi h,seconds_in_8_hrs+3
        call add4
nh08:   mov a,e
        ani 04h
        jz nh04
        lxi h,seconds_in_4_hrs+3
        call add4
nh04:   mov a,e
        ani 02h
        jz nh02
        lxi h,seconds_in_2_hrs+3
        call add4
nh02:   mov a,e
        ani 01h
        jz nh01
        lxi h,seconds_in_1_hr+3
        call add4
nh01:   

; same for minute
; look at each bit, add number of seconds
        lhld p10
        lxi d,5
        dad d           ; point to minute
        mov e,M
        mov a,e
        ani 20h         ; 32
        jz nm20
        lxi h,seconds_in_32_min+3
        call add4
nm20:   mov a,e
        ani 10h         ; 16
        jz nm10
        lxi h,seconds_in_16_min+3
        call add4
nm10:   mov a,e
        ani 08h
        jz nm08
        lxi h,seconds_in_8_min+3
        call add4
nm08:   mov a,e
        ani 04h
        jz nm04
        lxi h,seconds_in_4_min+3
        call add4
nm04:   mov a,e
        ani 02h
        jz nm02
        lxi h,seconds_in_2_min+3
        call add4
nm02:   mov a,e
        ani 01h
        jz nm01
        lxi h,seconds_in_1_min+3
        call add4
nm01:   

; finally, add the number of seconds.
        lhld p10
        lxi d,6
        dad d           ; seconds
        mov a,M
; (make a 4-byte number out of seconds)
        lxi h,unit_seconds+3  ; (has high 3 bytes 0)
        mov M,a
        call add4

; append the 2-byte fraction to temp6, as-is
        lhld p10
        lxi d,7
        dad d           ; HL points to byte with 1/256 sec
        mov a,M
        sta temp6+4
        inx h
        mov a,M
        sta temp6+5
         
; resulting 6-byte time is 4 bytes of seconds
; and 2 bytes of fraction.
; Copy result to caller
        lhld p6         ; (destination may overlap source)
        xchg            ; DE points to caller's result
        lxi h,temp6
        mvi c,6
        call COPY_BYTES

        ret


; do a 4-byte add
add4:           ; HL - points to low byte of addend
        push psw

        lda temp6+3
        add M             
        sta temp6+3
        dcx h

        lda temp6+2
        adc M           ; add with carry 
        sta temp6+2
        dcx h

        lda temp6+1
        adc M           ; add with carry 
        sta temp6+1
        dcx h

        lda temp6+0
        adc M           ; add with carry 
        sta temp6+0

        pop psw
        ret


; do a 4-byte subtract
sub4:           ; HL - points to low byte of subtrahend

        lda temp6+3
        sub M             
        sta temp6+3
        dcx h

        lda temp6+2
        sbb M           ; subtract with borrow
        sta temp6+2
        dcx h

        lda temp6+1
        sbb M           ; subtract with borrow
        sta temp6+1
        dcx h

        lda temp6
        sbb M           ; subtract with borrow
        sta temp6

; test for negative
        lda temp6
        ani 80h

        ret

C_6_TO_10:
; HL - points to 6-byte time
; DE - points to 10-byte result

; save pointers
        shld p6
        xchg
        shld p10

; copy 6-byte time to a temporary
; (so source and destination can overlap)
        lhld p6
        lxi d,temp6
        mvi c,6
        call COPY_BYTES

; generate year
        lxi d,2000      ; year starts at 2000
yyl1:   lxi h,seconds_in_a_year+3
        mov a,e         ; year is multiple of 4?
        ani 3
        jnz notleapyear
        lxi h,seconds_in_a_leapyear+3
notleapyear:
        shld year_seconds
        call sub4
        jnz yov         ; negative result?
        inx d           ; no, count up a year
        jmp yyl1
; overdraw; add back
yov:    lhld year_seconds
        call add4
; store DE as UT year
        lhld p10
        mov M,d
        inx h
        mov M,e

; generate day number
        lxi d,1         ; day starts at 1
dd1:    lxi h,seconds_in_256_days+3
        call sub4
        jnz dov1
        inr d           ; increment high byte
        jmp dd1 
dov1:   lxi h,seconds_in_256_days+3
        call add4

dd16:   lxi h,seconds_in_16_days+3
        call sub4
        jnz dov16

; add 16 to DE
        push h
        lxi h,16
        dad d
        xchg
        pop h

        jmp dd16
dov16:  lxi h,seconds_in_16_days+3
        call add4  ; add back


dd2:    lxi h,seconds_in_1_day+3
        call sub4
        jnz dov2
        inx d           
        jmp dd2
dov2:   lxi h,seconds_in_1_day+3
        call add4
; store day number
        lhld p10
        inx h
        inx h
        mov M,d
        inx h
        mov M,e

; Hour
        mvi c,0         ; Hour starts at 0
hh1:    lxi h,seconds_in_1_hr+3
        call sub4
        jnz hov1
        inr c           ; increment hour
        jmp hh1
hov1:   lxi h,seconds_in_1_hr+3
        call add4       ; add back
; store hour
        lhld p10
        lxi d,4
        dad d
        mov M,c

; Minute
        mvi c,0
mm1:    lxi h,seconds_in_1_min+3
        call sub4
        jnz mov1
        inr c
        jmp mm1
mov1:   lxi h,seconds_in_1_min+3
        call add4
; store minute
        lhld p10
        lxi d,5
        dad d
        mov M,c
        
; second is all that's left
        lhld p10
        lxi d,6
        dad d
        lda temp6+3     ; remaining seconds
        mov M,a         ; store seconds

; Finally, store the 2 fraction of seconds byte
        lda temp6+4
        inx h
        mov M,a
        inx h
        lda temp6+5
        mov M,a

; store a zero in the filler
        inx h
        mvi a,0
        mov M,a

        ret

; (Generated by C program "n.c")
seconds_in_a_year db 001h,0E1h,033h,080h
seconds_in_a_leapyear db 001h,0E2h,085h,000h
seconds_in_256_days db 001h,051h,080h,000h
seconds_in_128_days db 000h,0A8h,0C0h,000h
seconds_in_64_days db 000h,054h,060h,000h
seconds_in_32_days db 000h,02Ah,030h,000h
seconds_in_16_days db 000h,015h,018h,000h
seconds_in_8_days db 000h,00Ah,08Ch,000h
seconds_in_4_days db 000h,005h,046h,000h
seconds_in_2_days db 000h,002h,0A3h,000h
seconds_in_1_day db 000h,001h,051h,080h
seconds_in_16_hrs db 000h,000h,0E1h,000h
seconds_in_8_hrs db 000h,000h,070h,080h
seconds_in_4_hrs db 000h,000h,038h,040h
seconds_in_2_hrs db 000h,000h,01Ch,020h
seconds_in_1_hr db 000h,000h,00Eh,010h
seconds_in_32_min db 000h,000h,007h,080h
seconds_in_16_min db 000h,000h,003h,0C0h
seconds_in_8_min db 000h,000h,001h,0E0h
seconds_in_4_min db 000h,000h,000h,0F0h
seconds_in_2_min db 000h,000h,000h,078h
seconds_in_1_min db 000h,000h,000h,03Ch


; PWR.A -- control of power services

ps_data  equ GBL_PWR   ; ds 3
cdi_send equ GBL_PWR+3 ; 4 bytes (call, ret)
cdi_ahl  equ GBL_PWR+7 ; 4 bytes (call, ret)
; 11 bytes

; (keep in sync with "CDI.I")  
PC_PS1500   equ 010h  ; Power Services 0..15
PC_PS16     equ 011h  ; Power Service 16

PWR_SET_ON:
; A - service number 0..16
        push psw
        push h
        cpi 16
        jz ps16on       ; check for PS16
        jp badsn        ; otherwise, should be 0..15
; set bit in mask
        lxi h,1
sbit:   cpi 0
        jz bitset
        dcr a
        dad h           ; shift bit left
        jmp sbit
; OR with current power word
bitset: lda ps_data+1     ; fetch high byte of power word
        ora h           ; OR with high byte of mask
        sta ps_data+1     ; replace high byte of power word
        lda ps_data+2   ; fetch low byte
        ora l           ; OR with low byte of mask
        sta ps_data+2   ; replace low byte
        lxi h,ps_data+1   ; point to 2 bytes of ps_data
        mvi a,PC_PS1500   ; CDI operation: set PS 0-15
        call CDI_SEND
        call wait_1ms
        jmp pxitok
ps16on: 
        lda ps_data+0 ; fetch state of PS16
        ori 1           ; or in a 1 bit
        sta ps_data+0 ; replace PS16
        lxi h,1
        mvi a,PC_PS16   ; CDI operation: set PS16
        call CDI_AHL
        call wait_1ms
pxitok: 
        pop h
        pop psw         ; A is nonzero; service was OK
        ret

badsn:  pop h
        pop psw         ; throw away A
        mvi a,0FFh      ; service number was invalid
        ret

PWR_SET_OFF:
; A - service number 0..16
        push psw
        push h
        cpi 16
        jz ps16off      ; check for PS16
        jp badsn        ; otherwise, should be 0..15
; set bit in mask
        lxi h,1
sbit0:  cpi 0
        jz bitset0
        dcr a
        dad h           ; shift bit left
        jmp sbit0
; And complement with current power word
bitset0:
        mov a,h
        xri 0FFh       ; complement -- flip bits
        mov h,a
        lda ps_data+1     ; fetch high byte of power word
        ana h           ; AND with high byte of mask
        sta ps_data+1     ; replace high byte of power word

        mov a,l
        xri 0FFh
        mov l,a
        lda ps_data+2   ; fetch low byte
        ana l           ; AND with low byte of mask
        sta ps_data+2   ; replace low byte

        lxi h,ps_data+1   ; point to 2 bytes of ps_data
        mvi a,PC_PS1500   ; CDI operation: set PS 0-15
        call CDI_SEND   
        jmp pxitok
ps16off: lda ps_data+0 ; fetch state of PS16
        ani 0FEh        ; turn off bit
        sta ps_data+0 ; replace PS16
        lxi h,0
        mvi a,PC_PS16   ; CDI operation: set PS16
        call CDI_AHL
        jmp pxitok

; CCSDS - generate CCSDS source packets

; RAM allocation
; (GBL_CCSDS allocates 100 bytes)

; "ssci" is a place to build primary and secondary headers
; for science packets
; plus 4 bytes of other stuff - total of 20 bytes   
ssci                equ GBL_CCSDS      ; ds 20  
; "ssoh" is a place to build primary and secondary headers
; for housekeeping packets - 16 bytes
ssoh                equ GBL_CCSDS+20   ; ds 16
ApID                equ GBL_CCSDS+36   ; ds 1
source_seq_count    equ GBL_CCSDS+37   ; ds 2
data_pointer        equ GBL_CCSDS+39   ; ds 2
mm_addr_of_data     equ GBL_CCSDS+41   ; ds 2
p_otherstuff        equ GBL_CCSDS+43   ; ds 2

clock_read          equ GBL_CCSDS+45   ; 3 bytes (jmp)
clock_6_to_10       equ GBL_CCSDS+48
soh_out             equ GBL_CCSDS+51
pvcf_dpu            equ GBL_CCSDS+54
task_new            equ GBL_CCSDS+57
;pwr_set_on          equ GBL_CCSDS+60
;pwr_set_off         equ GBL_CCSDS+63
pvcf_mm             equ GBL_CCSDS+66
task                equ GBL_CCSDS+69


; Generate packet and send it out the SOH port
Q_CCSDS_HSKP:
;   A  - ApID minus 0x700
;   BC - total packet length
;   DE - source sequence count for this ApID
;   HL - address of data, starting AFTER 2ndary header 

; generate and transmit primary header
        push h          ; preserve data pointer
        push b          ; preserve length
        lxi h,ssoh
        call make_primary_header  ; store at "ssoh"
; transmit primary header
        mvi c,6         ; 6 bytes
        call xmt_to_SOH

; Now send the Secondary Header - time stamp
        lxi h,ssoh+6
        call CLOCK_READ ; read current time
; Convert to 10-byte form
        lxi d,ssoh+6       ; result replaces input
        call CLOCK_6_TO_10

; calculate the checksum of the data
        mvi e,0         ; clear checksum accumulator

; send and checksum the Secondary Header
        mvi c,10
        call xmt_to_SOH

; now send and checksum the data 
        pop b           ; total packet length
        pop h           ; data pointer
        mov a,c         ; (SOH packets are less than 255 length)
; deduct pri hdr(6), 2ndry hdr(10), cksum(1), trailer(1)
        sui 6+10+2 
        mov c,a
        call xmt_to_SOH
 
; Send the checksum
        mov a,e
        call SOH_OUT

; Finally, send the Trailer
        mvi a,0AAh
        call SOH_OUT

        ret

; send byte string out via SOH port
;  HL - points to byte string
;   C - byte count
;   E - accumulated checksum; returned updated
xmt_to_SOH:
        push psw
        push b
        push h
cs1:    mov a,M
        call SOH_OUT
        xra e           ; include in checksum
        mov e,a
        inx h
        dcr c
        jnz cs1
        pop h
        pop b
        pop psw
        ret


; Generate packet and send it out the science channel
Q_CCSDS_SCIHDR:

;   A  - ApID minus 0x700
;   BC - total packet length
;   DE - source sequence count for this ApID
;   HL - address of data, starting AFTER 2ndary header 

        push psw        ; >> preserve ApID

        lda GBL_TM_Status
        cpi 1           ; TM being generated?
        jz xtm2
;  no, can't send on science channel
        pop psw
        ret
xtm2:   shld data_pointer
        pop psw         ; restore ApID
; Generate packet and store it in the PVCF array
; generate primary header
        lxi h,ssci
        call make_primary_header   ; (result in s)
; Now generate the Secondary Header - time stamp
        lxi h,ssci+6 
        call CLOCK_READ ; read current time
; Convert to 10-byte form
        lxi d,ssci+6       ; result replaces input
        call CLOCK_6_TO_10
; send to PVCF-generator
        lhld data_pointer
        xchg            ; data pointer in DE
        lxi h,ssci
;  HL - points to headers
;  DE - points to data
        call PVCF_DPU
        ret

                




; generate a science data packet
;  A - ApID minus 0x700
;  BC - ApID's source sequence number
;  DE - points to MM address of data
;  HL - points to "other stuff" in packet before data
;          2 bytes of data packet sequence number
;          2 bytes of number-of-valid-words
Q_CCSDS_SCIDATA:

        push psw        ; >> preserve ApID
        shld p_otherstuff
; generate primary header
        xchg
        shld mm_addr_of_data
        mov d,b         ; DE=BC (source seq count)
        mov e,c
        lxi b,4094      ; total length of a data packet
        pop psw         ; << restore ApID 
        lxi h,ssci         ; where to build the header
        call make_primary_header  
; Now generate the Secondary Header - time stamp
        lxi h,ssci+6 
        call CLOCK_READ ; read current time
; Convert to 10-byte form
        lxi d,ssci+6       ; result replaces input
        call CLOCK_6_TO_10

; append "other stuff" to headers
        lhld p_otherstuff
        lxi d,ssci+16
        mvi c,4
        call copy_bytes

; send to PVCF-generator
        lhld mm_addr_of_data
        xchg            ; put MM addr in DE
        lxi h,ssci
;  HL - points to headers plus other stuff
;  DE - points to MM address of data
        call PVCF_MM
        ret


make_primary_header:
; (re-entrant)
; A  - ApID minus 0x700
; BC - total packet length
; DE - source sequence count
; HL - where to store 6-byte header
        push psw
        push b
        push d
        push h

; Byte 0
;  3 bits of version number = 000, fixed                                      
;  1 bit of type = 0, fixed                                             
;  1 bit showing presence of secondary header = 1, fixed                        
;  3 bits which are the MSB's of the APID = 111, fixed
;    (all ISUAL ApIDs are 7XX)
        mvi M,0Fh
        inx h

; Byte 1
;  8 bits which are the LSB's of the APID.                                    
        mov M,a
        inx h

; Byte 2
;  2 bits = 11, fixed, which show that segmentation is not being used   
;  6 bits which are the MSB's of the source sequence count (variable)        
        mov a,d         ; MSB of source seq count
        ani 03Fh        ; mask to 14 bits
        ori 0C0h        ; merge in '11'
        mov M,a
        inx h

; Byte 3
;   8 bits which are the LSB's of the source sequence count  (variable)        
        mov M,e         ; LSB of source seq count
        inx h

; Length of the Packet Data Field                                   
;  portion of the packet.
; The value includes the Secondary Header 
;  and the Trailer, minus 1.
;  BC=Total_packet_length
        xchg
        lxi h,-7   ;  deduct length of primary hdr +1
        dad b

; Byte 4
;   8 bits which are the MSB's of the data length
        xchg
        mov M,d
        inx h

; Byte 5
;   8 bits which are the LSB's of the data length.  
        mov M,e

        pop h
        pop d
        pop b
        pop psw
        ret



; Control of Heaters
GH  equ GBL_HTR_WORK

; There are 5 Heaters controlled by the DPU
N_HTRS  equ 5

; Each heater has 5 bytes associated with it:
H_STATUS   equ GH    ; ds 1 ;  FF=inactive, 1=on, 0=off
H_SERVICE  equ GH+1  ; ds 1 ;  power service number
H_CTR      equ GH+2  ; ds 1 ;  decremented to zero, then state changes
H_SECS_ON  equ GH+3  ; ds 1 ;  how many seconds heater should be on
H_SECS_OFF equ GH+4  ; ds 1 ;  how many seconds heater should be off
H_SIZE  equ 5 ; size of array element

heater_table equ GH+5 ;  ds N_HTRS*H_SIZE
S0         equ GH+30 ; ds 1
hcount     equ GH+31 ; ds 1
pelt       equ GH+32 ; ds 2
onsecs     equ GH+35 ; ds 1
offsecs    equ GH+36 ; ds 1

;  GBL.I allocates 40 bytes to GBL_HTR_WORK

; The heater task checks the list of heaters
; every second.  For active heaters, it decrements
; nsecs_left; if zero, it either turns the heater
; on or off, and stores either nsecs_on or nsecs_off
; in nsecs_left

Q_HTR_INIT:
; initialize heater array
        lxi h,heater_table
        mvi M,0FFh      ; inactive
        inx h
; 21-Dec-2002 changed Front Lens Header from PS 7 to 8
        mvi M,8         ; heater 1 - Imager Front Lens - service 8
        lxi d,H_SIZE-1
        dad d           
        mvi M,0FFh      ; inactive
        inx h
        mvi M,12        ; heater 2 - Imager Rear Lens - service 12
        lxi d,H_SIZE-1
        dad d           
        mvi M,0FFh      ; inactive
        inx h
        mvi M,15        ; heater 3 - Imager Filter - service 15
        lxi d,H_SIZE-1
        dad d           
        mvi M,0FFh      ; inactive
        inx h
        mvi M,6         ; heater 4 - SP Filter - service 6
        lxi d,H_SIZE-1
        dad d           
        mvi M,0FFh      ; inactive
        inx h
        mvi M,14        ; heater 5 - SP Body - service 14

; Start Heater Task
        lxi d,heater_task
        lxi h,GBL_HTR_STACK+stack_size
        call task_new

        ret

heater_task:
        lda GBL_SECONDS_CTR
        sta S0

sloop:  call task
        lda S0
        mov c,a
        lda GBL_SECONDS_CTR
        cmp c
        jz sloop

; New seconds
        sta S0  ; new seconds
; do a scan of the heaters
        lxi h,heater_table
        shld pelt
        mvi a,N_HTRS
        sta hcount

hloop:  lhld pelt
        mov a,M
        cpi 0FFh
        jnz active
; heater is inactive
        lxi d,H_SIZE    ; step to next heater
        dad d
        shld pelt
        lda hcount
        dcr a
        sta hcount
        jnz hloop
        jmp sloop       ; next second

; Heater is Active
active:
; Decrement counter
        lxi d,H_CTR-H_STATUS
        dad d
        dcr M           ; decrement counter
        jz expired
; counter still not zero, step to next heater
        lhld pelt
        lxi d,H_SIZE
        dad d
        shld pelt
        lda hcount
        dcr a
        sta hcount
        jnz hloop        ; next heater

        jmp sloop        ; next second 


; counter went to zero
expired:
; copy element to a place with labels
        lhld pelt
        lxi d,H_STATUS
        mvi c,H_SIZE
        call copy_bytes
; Turn Heater on or off
        lda H_STATUS
        cpi 1
        jz was_on
; was off
        mvi a,1         ; turn on
        sta H_STATUS
        lda H_SERVICE   ; turn power service on
        call PWR_SET_ON
        lda H_SECS_ON
        sta H_CTR       ; set counter
        jmp end_of_changes
was_on:
        mvi a,0         ; turn off
        sta H_STATUS
        lda H_SERVICE   ; turn power service off
        call PWR_SET_OFF
        lda H_SECS_OFF
        sta H_CTR       ; set counter

end_of_changes:
; copy element back to array
        lxi d,H_STATUS  ; source
        lhld pelt       ; destination
        xchg
        mvi c,H_SIZE
        call copy_bytes ; DE and HL returned advanced
        xchg            ; HL points to next element
        shld pelt
next:   lda hcount
        dcr a
        sta hcount
        jnz hloop       ; next heater

        jmp sloop       ; next second

       
        
Q_HTR_SET:
; A - which heater (1..5)
; B - 1=activate, 0=deactivate
; C - ON_SECS
; D - OFF_SECS
        push psw
        push b
        push d
        push h

        push psw
        mov a,c
        sta onsecs
        mov a,d
        sta offsecs
        pop psw

; Locate heater element
        lxi h,heater_table
ll:     dcr a
        jz gotit
        lxi d,H_SIZE
        dad d
        jmp ll
gotit:
        mov a,b
        cpi 1
        jz activate
; deactivate heater
        mvi M,0FFH      ; status=deactivated
        lxi d,H_SERVICE-H_STATUS
        dad d
        mov a,M         ; power service
        call PWR_SET_OFF
        jmp xit
; Activate Heater
activate:
;  setup heater to be turned on next cycle
        mvi M,0         ; state=off
        push h
        lxi d,H_CTR-H_STATUS
        dad d
        mvi M,1         ; counter=1 second
; set SECS_ON and SECS_OFF
        pop h
        push h
        lxi d,H_SECS_ON-H_STATUS
        dad d
        lda onsecs
        mov M,a
        pop h
        lxi d,H_SECS_OFF-H_STATUS
        dad d
        lda offsecs
        mov M,a

xit:    pop h
        pop d
        pop b
        pop psw
        ret


; extension of "execute.a" for more commands
q_exec2:
; A - opcode
        cpi 050h ;  I_WAIT_MON
         jz wait_mon
        cpi 051h ;  I_WAIT_SOH_BYTE
         jz wait_soh_byte
        cpi 052h ;  I_WAIT_SOH_WORD
         jz wait_soh_word
        cpi 080h ; I_JUMPTO
         jz jumpto
        cpi 025h ;  I_TLM_DIRECT
         jz tlm_direct
        cpi 063h ;  I_TLM_DIRECT_ALL
         jz tlm_direct_all
        cpi 30h   ; I_DSENS_FORCE_ON
         jz dsens_on
        cpi 29h   ; I_DSENS_FORCE_OFF
         jz dsens_off
        cpi 04h  ;  I_HV_MAYI
         jz hv_mayi
        cpi 053h ;  I_TIMEOUT_SET
         jz timeout_set
        cpi 031h ; I_CAM_STIMS_ON
         jz cam_stims_on
        cpi 032h ; I_CAM_STIMS_OFF
         jz cam_stims_off
        cpi 033h ; I_SP1_STIMS_ON
         jz sp1_stims_on
        cpi 034h ; I_SP1_STIMS_OFF
         jz sp1_stims_off
        cpi 085h ; I_SP2_STIMS_ON
         jz sp2_stims_on
        cpi 036h ; I_SP2_STIMS_OFF
         jz sp2_stims_off
        cpi 086h ; I_SP3_STIMS_ON
         jz sp3_stims_on
        cpi 087h ; I_SP3_STIMS_OFF
         jz sp3_stims_off
        cpi 07Fh; I_SP4_STIMS_ON  ; (v71 - was 10h)
         jz sp4_stims_on
        cpi 01Bh ; I_SP4_STIMS_OFF
         jz sp4_stims_off
        cpi 01Fh ; I_SP5_STIMS_ON
         jz sp5_stims_on
        cpi 028h ; I_SP5_STIMS_OFF
         jz sp5_stims_off
        cpi 055h ; I_SP6_STIMS_ON
         jz sp6_stims_on
        cpi 056h ; I_SP6_STIMS_OFF
         jz sp6_stims_off
        cpi 088h ; I_HV_SP_AURORA_SET
         jz hv_sp_aurora_set
        cpi 089h ; I_HV_SP_SPRITE_SET
         jz hv_sp_sprite_set
        cpi 08Ah ; I_HV_MCP_SELECT
         jz hv_mcp_select

        cpi 03Bh ;  I_CLEAR_STATS
         jz clear_stats
        cpi 03Ah ;  I_CLEAR_FAULTS
         jz clear_faults
        cpi 08Bh ;  I_HV_PROTECTION_ENABLE
         jz enhvp
        cpi 08Ch ;  I_HV_PROTECTION_DISABLE
         jz dshvp
        cpi 08Dh ;  I_ECHO_ENABLE
         jz echoen
        cpi 08Eh ;  I_ECHO_DISABLE
         jz echods
        cpi 045h ;  I_DSENS_SET 
         jz dsens_set
        cpi 018h ;  I_CAM
         jz icam
        cpi 019h ;  I_SPF
         jz ispf
        cpi 01Ah ;  I_AP
         jz iap
        cpi 024h ;  I_GENERIC_CMD
         jz generic_cmd
        cpi 17h  ; I_TLM_FLUSH
         jz tlm_flush

        cpi 040h ;  I_CAM_HV_ON
         jz cam_hv_on
        cpi 041h ;  I_SPA_HV_ON
         jz spa_hv_on
        cpi 042h ;  I_SPB_HV_ON
         jz spb_hv_on

        mvi a,2         ; bad OP
        ret

exitok: mvi a,0
exit:   ret

; I_JUMPTO BANK=X ADDR=XX
jumpto: mov a,M
        out BANK_SELECT_O
        inx h
        mov d,M  
        inx h
        mov e,M
; jump to the address
        xchg    ; HL=DE
        pchl    ; go to (HL)

; I_CAM_STIMS_ON
cam_stims_on:
        in STIM_CTL_I
        ori 40h         ; b6
        out STIM_CTL_O
        jmp exitok
; I_CAM_STIMS_OFF
cam_stims_off
        in STIM_CTL_I
        ani 0BFh  
        out STIM_CTL_O
        jmp exitok

; I_SP1_STIMS_ON
sp1_stims_on:
        in STIM_CTL_I
        ori 01h         ; b0
        out STIM_CTL_O
        jmp exitok
; I_SP1_STIMS_OFF
sp1_stims_off
        in STIM_CTL_I
        ani 0FEh         ; b0
        out STIM_CTL_O
        jmp exitok

; I_SP2_STIMS_ON
sp2_stims_on:
        in STIM_CTL_I
        ori 02h         ; b1
        out STIM_CTL_O
        jmp exitok
; I_SP2_STIMS_OFF
sp2_stims_off
        in STIM_CTL_I
        ani 0FDh         ; b1
        out STIM_CTL_O
        jmp exitok

; I_SP3_STIMS_ON
sp3_stims_on:
        in STIM_CTL_I
        ori 04h         ; b2
        out STIM_CTL_O
        jmp exitok
; I_SP3_STIMS_OFF
sp3_stims_off
        in STIM_CTL_I
        ani 0FBh         ; b2
        out STIM_CTL_O
        jmp exitok

; I_SP4_STIMS_ON
sp4_stims_on:
        in STIM_CTL_I
        ori 08h         ; b3
        out STIM_CTL_O
        jmp exitok
; I_SP4_STIMS_OFF
sp4_stims_off
        in STIM_CTL_I
        ani 0F7h 
        out STIM_CTL_O
        jmp exitok

; I_SP5_STIMS_ON
sp5_stims_on:
        in STIM_CTL_I
        ori 010h         ; b4
        out STIM_CTL_O
        jmp exitok
; I_SP5_STIMS_OFF
sp5_stims_off
        in STIM_CTL_I
        ani 0EFh
        out STIM_CTL_O
        jmp exitok

; I_SP6_STIMS_ON
sp6_stims_on:
        in STIM_CTL_I
        ori 20h         ; b5
        out STIM_CTL_O
        jmp exitok
; I_SP6_STIMS_OFF
sp6_stims_off
        in STIM_CTL_I
        ani 0DFh        
        out STIM_CTL_O
        jmp exitok

; I_TLM_DIRECT APID=XX VIA=X
; Direct certain telemetry ApIDs to SOH, TLM, both or neither
tlm_direct:
        inx h           ; ignore high byte 
        mov a,M         ; ApID minus 700h
        inx h           ; point to VIA
; select the ApID
        cpi 03h         ; 703
        jz d703
        cpi 05h         ; 705
        jz d705
        cpi 1Ch
        jz d71C
        cpi 1Eh         ; 71E (event packet)
        jz d71E
        cpi 1Dh
        jz d71D
        cpi 09h
        jz d709
        cpi 19h
        jz d719
        cpi 20h
        jz d720
        cpi 1Ah
        jz d71A
        cpi 1Bh
        jz d71B
        cpi 0Bh
        jz d70B
        jmp exitok ; (shrug)

; set the appropriate code
d703:   mov a,M         ; get code
        sta GBL_703     ; store in director-flag
        jmp exitok
d705:   mov a,M         ; get code
        sta GBL_705     ; store in director-flag
        jmp exitok
d71C:   mov a,M         ; get code
        sta GBL_71C     ; store in director-flag
        jmp exitok
d71D:   mov a,M         ; get code
        sta GBL_71D     ; store in director-flag
        jmp exitok
d71E:   mov a,M         ; get code
        sta GBL_71E     ; store in director-flag
        jmp exitok
d709:   mov a,M         ; get code
        sta GBL_709     ; store in director-flag
        jmp exitok
d719:   mov a,M         ; get code
        sta GBL_719     ; store in director-flag
        jmp exitok
d720:   mov a,M         ; get code
        sta GBL_720     ; store in director-flag
        jmp exitok
d71A:   mov a,M         ; get code
        sta GBL_71A     ; store in director-flag
        jmp exitok
d71B:   mov a,M         ; get code
        sta GBL_71B     ; store in director-flag
        jmp exitok
d70B:   mov a,M         ; get code
        sta GBL_70B     ; store in director-flag
        jmp exitok


; I_TLM_DIRECT_ALL VIA=X
; Direct all SOH telemetry ApIDs to SOH, TLM, both or neither
tlm_direct_all:
; set the appropriate code
        mov a,M          ; get code
        sta GBL_703     ; store in director-flag
        sta GBL_705
        sta GBL_70B
        sta GBL_71A
        sta GBL_71B
        sta GBL_71C     
        sta GBL_71D
        sta GBL_71E
        sta GBL_709     
        sta GBL_719
        jmp exitok

; I_DSENS_FORCE_ON
; simulate daylight
dsens_on:
        in DAY_SENS_STS_I
        ori 80h                 ; b7=1
        jmp senso               ; don't enable interrupts (b1,b0)

; I_DSENS_FORCE_OFF
; don't simulate daylight
dsens_off:
        in DAY_SENS_STS_I
        ani 7Ch                 ; b7=0
        jmp senso


; I_HV_MAYI b1 b2 b3 b4
; Set the "magic" 4 bytes which enable writing
; to an HV register
hv_mayi:
        mov a,M
        out HV_UNLOCK_3_O  ; b1, written first
        inx h
        mov a,M         
        out HV_UNLOCK_2_O  ; b2
        inx h
        mov a,M         
        out HV_UNLOCK_1_O  ; b3  
        inx h
        mov a,M         
        out HV_UNLOCK_0_O  ; b4, written last
        jmp exitok

; I_HV_SP_AURORA_SET
hv_sp_aurora_set:
        in HV_STS_I
        ori 80h         ; b7=1 -- select SP Aurora mode
        out HV_CTL_O
        jmp exitok

; I_HV_SP_SPRITE_SET
hv_sp_sprite_set:
        in HV_STS_I
        ani 7Fh         ; b7=0 -- select SP Sprite mode
        out HV_CTL_O
        jmp exitok

; I_HV_MCP_SELECT SETTING=X
hv_mcp_select:
        mov a,M         ; SETTING
        cpi 0
        jz badp
        cpi 8
        jp badp         ; must be 1..7
; shift left to bits 4..6
        ral
        ral
        ral
        ral
        ani 70h         ; bits 4..6
        mov c,a
        in HV_STS_I     ; get status
        ani 8Fh         ; zero bits 4..6
        ora c           ; OR-in the new bits
        out HV_CTL_O
        jmp exitok
badp:   mvi a,1         ; ERR_BADP - bad parameter
        ret

; I_CLEAR_FAULTS
clear_faults:
        mvi a,0
        sta GBL_SOH+12  ; "at least one fault"
        sta GBL_SOH+29  ; DCM fault
        sta GBL_SOH+33  ; TPA fault
        sta GBL_SOH+75  ; Buffer fault
        sta GBL_SOH+76  ; MBR fault
        sta GBL_SOH+102 ; Filter Wheel fault
        sta GBL_SOH+105 ; PPS faults
        sta GBL_SOH+165 ; cam supply Q fault
        sta GBL_SOH+166 ; spf supply Q fault
        sta GBL_SOH+167 ; sps supply Q fault
        sta GBL_SOH+168 ; aph supply Q fault
        jmp exitok

; I_CLEAR_STATS
clear_stats:
        mvi a,0
        lxi h,0
        sta GBL_SOH+19  ; no. PVCFs sent
        sta GBL_SOH+23  ; no. Sprite/Lightning Events
        shld GBL_SOH+27 ; no. of MMCBs needing compression
        shld GBL_SOH+31 ; no. TPAs received
        sta GBL_SOH+34  ; no. CMD_IN overruns
        sta GBL_SOH+35  ; no. bad packet lengths
        sta GBL_SOH+36  ; no. missing packet trailers
        shld GBL_SOH+37 ; no. commands received
        sta GBL_SOH+37  ; no. commands received
        sta GBL_SOH+49  ; no. commands with bad ApID
        sta GBL_SOH+50  ; no. commands with bad arg value
        sta GBL_SOH+100 ; no. time-sets
        shld GBL_SOH+51 ; no. commands successfully executed
        shld GBL_SOH+63 ; no. time-tagged cmds rec'd
        shld GBL_SOH+65 ;   "            "       "   exec'd
        shld GBL_SOH+67 ; no. active elements in TT list
        sta GBL_SOH+69  ; no. HV Protection trips
        shld GBL_SOH+71 ; no. PVCFs in SC buffer #1
        shld GBL_SOH+73 ; no. PVCFs in SC buffer #2
        shld GBL_SOH+149 ; MM EDAC counters
        shld GBL_SOH+151
        sta GBL_SOH+106 ; no. commands with invalid opcodes
        sta GBL_SOH+107 ; no. commands with bad checksum
        sta GBL_SOH+112 ; no. "noise" bytes in CMD stream
        jmp exitok


; I_HV_PROTECTION_DISABLE
; disable HV protection
dshvp:  in SCIF_STS_I
        ori 08h         ; b3=1
        out SCIF_CTL_O
        jmp exitok

; I_HV_PROTECTION_ENABLE
enhvp:  in SCIF_STS_I
; enable HV protection
        ani 0F7h        ; b3=0
        out SCIF_CTL_O
        jmp exitok

; I_ECHO_ENABLE
echoen: mvi a,0
        sta GBL_ECHO_DISABLE
        jmp exitok

; I_ECHO_DISABLE
echods: mvi a,1
        sta GBL_ECHO_DISABLE
        jmp exitok

; I_DSENS_SET MASK=X
dsens_set:
        mov a,M         ; MASK
        ani 30h         ; allow only b4 and b5
        mov c,a
        in DAY_SENS_STS_I
        ani 0CFh        ; zero b4, b5
        ora c           ; merge in new bits
; zero the TBD bits and (b1,b0) - interrupt enables
senso:  ani 0B0h        ; b6,b3,b2,b1,b0=0 
        out DAY_SENS_CTL_O
        jmp exitok

mon_min equ GBL_EEPROM     ; ds 1
mon_max equ GBL_EEPROM+1   ; ds 1
timeout  equ GBL_WAIT_TIMEOUT

; I_WAIT_MON offset=XX min=X max=X
wait_mon:
; get offset of monitor item
        mov d,M         ; get 16-bit offset
        inx h
        mov e,M         ; DE=offset
        inx h
; get max and min
        mov a,M         ; min
        sta mon_min
        inx h
        mov a,M         ; max [***2-May-2003]
        sta mon_max
        lxi h,GBL_MON   ; base of Monitor array
        dad d           ; add offset
; *** 2-May-2003
; HL now points to desired MON byte
        jmp cwait
; ***
; Check for Monitor value within a range
;  min <= mon <= max
;wm1:    call task
;        lda mon_min
;        sub M           ; min-mon
;        ani 80h         ; check sign of result
;        jz wm2          ; mon < min
;        lda mon_max
;        sub M           ; max-mon
;        jnz wm2         ; mon > max
;        jmp exitok
; value still out of range              
; check timeout
;wm2:    lda GBL_SECONDS_CTR
;        sub c           ; diff=now-start
;        sub b           ; diff-timeout
;        ani 80h
;        jnz wm1         ; diff<timeout
; timeout expired
;        mvi a,9 ; ERR_TIMEOUT
;        jmp exit
; ***


; I_WAIT_SOH_BYTE offset=XX min=X max=X
wait_soh_byte:
; get addr of SOH item
        mov d,M         ; get 16-bit offset
        inx h
        mov e,M         ; DE=offset
        inx h
; get max and min
        mov a,M         ; min
        sta mon_min
        inx h
        mov a,M
        sta mon_max
        lxi h,GBL_SOH   ; base of SOH array
        dad d           ; add offset
; HL now points to desired SOH byte
cwait:  lda timeout     ; set official timeout (seconds)
        mov b,a
        lda GBL_SECONDS_CTR     ; initial seconds
        mov c,a
; Check for SOH value within a range
;  min <= mon <= max
        lda mon_min
        mov d,a
        lda mon_max
        mov e,a
wb1:    call task
        mov a,M         ; mon
        sub d           ; mon-min
        ani 80h
        jnz wb2         ; mon<min
; mon>=min
        mov a,M         ; mon
        sub e           ; mon-max
        jz exitok       ; mon=max
        ani 80h
        jz wb2          ; mon>max
;  min <= mon <= max
        jmp exitok
; value still out of range              
; check timeout
wb2:    lda GBL_SECONDS_CTR
        sub c           ; diff=now-start
        sub b           ; diff-timeout
        ani 80h
        jnz wb1         ; diff<timeout
; timeout expired
        mvi a,9 ; ERR_TIMEOUT
        jmp exit



w_addr  equ GBL_EEPROM+2 ; ds 2
w_max   equ GBL_EEPROM+4 ; ds 2
w_min   equ GBL_EEPROM+6 ; ds 2

; I_WAIT_SOH_WORD offset=XX min=XX max=XX
wait_soh_word:
        mov d,M         ; get 16-bit offset
        inx h
        mov e,M         ; DE=offset
        push d
        inx h
        mov d,M         ; min
        inx h
        mov e,M
        push d
        inx h
        mov d,M
        inx h
        mov e,M
        shld w_max
        pop h
        shld w_min
        pop h
        lxi d,GBL_SOH
        dad d
        shld w_addr
        lda timeout     ; set official timeout (seconds)
        mov b,a
        lda GBL_SECONDS_CTR     ; initial seconds
        mov c,a
; test-loop
; we want:  min <= soh <= max
w_loop: call task
        lhld w_addr
        mov d,M         ; soh value
        inx h
        mov e,M
        xchg
        lhld w_min
        call sub16      ; min-soh
        mov a,h
        ani 80h         ; check sign of result
        jz w_2          ; soh < min
        lhld w_addr
        mov d,M         ; soh value
        inx h
        mov e,M
        xchg
        lhld w_max
        call sub16      ; max-soh
        mov a,h
        ani 80h
        jnz w_2         ; soh > max
;  min <= soh <= max  
; We've got it.
        jmp exitok
; value still out of range              
; check timeout
w_2:    lda GBL_SECONDS_CTR
        sub c           ; diff=now-start
        sub b           ; diff-timeout
        ani 80h
        jnz w_loop      ; diff<timeout
; timeout expired
        mvi a,9 ; ERR_TIMEOUT
        jmp exit


; I_TIMEOUT_SET seconds=X
timeout_set:
        mov a,M
        sta timeout
        jmp exitok


; I_CAM ENABLE=X
; Activate/Deactivate Camera data taking
icam:   mov a,M         ; get enable/disable code
        cpi 0           ; 0=disable
        jz cam_d
; enable camera
        lda GBL_SOH+11
        ori 08h         ; lite bit 3
        sta GBL_SOH+11
        jmp exitok
; disable camera
cam_d:  lda GBL_SOH+11
        ani 0F7h         ; bit 3=0
        sta GBL_SOH+11
        jmp exitok

; I_SPF ENABLE=X 
; Activate/Deactivate (Fast) Spectrophotometer 
ispf:   mov a,M
        cpi 0           ; 0=disable
        jz spf_d
; enable processing of SP Fast readouts only
        lda GBL_SOH+11
        ori 04h         ;  bit 2 =1
        sta GBL_SOH+11
        jmp exitok
; disable SPF 
spf_d:  lda GBL_SOH+11
        ani 0Bh
        sta GBL_SOH+11  ; software
        jmp exitok

; I_AP ENABLE=X
; Activate Array Photometer data taking
iap:    mov a,M
        cpi 0           ; 0=disable
        jz aph_d
        lda GBL_SOH+11
        ori 01h         ; lite bit 0
        sta GBL_SOH+11
        jmp exitok
; disable AP
aph_d:  
        lda GBL_SOH+11
        ani 0FEh         ; bit 0=0
        sta GBL_SOH+11
        jmp exitok


; Function to look at low 8 bits of ApID (in register A)
; and return HL with a pointer to a byte
; which will direct where the packet should be sent
go_soh  db 0 ; send to SOH
go_sci  db 1 ; send to science
Q_GET_SOHSCI:
; Some packets go out only via SOH
        lxi h,GO_SOH
        cpi 01h         ; 701 - MBR
        rz
        cpi 18h         ; 718 - Spacecraft Alert
        rz 
        cpi 1Fh         ; 71F - Group Execute Abort
        rz

; some packets may be directed out via TM or SOH (or both)
        lxi h,GBL_703
        cpi 3 
        rz
        lxi h,GBL_705
        cpi 5
        rz
        lxi h,GBL_71C
        cpi 1Ch
        rz 
        lxi h,GBL_71D
        cpi 1Dh
        rz 
        lxi h,GBL_720
        cpi 20h         ; 720 - the debug packet
        rz
        lxi h,GBL_71E
        cpi 1Eh
        rz
        lxi h,GBL_709
        cpi 09h
        rz
        lxi h,GBL_70B
        cpi 0Bh 
        rz
        lxi h,GBL_719
        cpi 19h
        rz
        lxi h,GBL_71A
        cpi 1Ah
        rz
        lxi h,GBL_71B
        cpi 1Bh
        rz
; if none of the above, send it out via science channel
        lxi h,GO_SCI
        ret

; (keep these in sync with CDI.I)
MM_IM_SIZEH   equ 0A2h
MM_IM_SIZEL   equ 0A3h
MM_AP_SIZE    equ 0A9h
MM_SPF_SIZE   equ 0ADh
MM_SPS_SIZE   equ 0B2h

q_set_rosizes:
; set the camera readout size into the hardware
        lxi h,GBL_CAM_NWRO    ; point to 4-byte size
; 21 bits of size
;    byte 0     byte 1     byte 2     byte 3
;   0000 0000  000x xxxx  yyyy yyyy  zzzz zzzz 
; send top 5 bits first
        inx h
        mvi d,0
        mov e,M                 ; 000x xxxx
        xchg
        mvi a,MM_IM_SIZEH
        call CDI_AHL            ; send top 5 bits 
; then bottom 16 bits
        xchg
        inx h
        mov d,M                 ; yyyy yyyy
        inx h
        mov e,M                 ; zzzz zzzz
        xchg
        mvi a,MM_IM_SIZEL
        call CDI_AHL            ; send bottom 16 bits

; set the SPF readout size into the hardware
        lxi h,GBL_SPF_NWRO+2  ; (won't exceed 16 bits)
        mov d,M
        inx h
        mov e,M
        xchg
        mvi a,MM_SPF_SIZE
        call CDI_AHL

; set the SPS readout size into the hardware
        lxi h,GBL_SPS_NWRO+2 ; (never gets bigger than 16 bits)
        mov d,M
        inx h
        mov e,M
        xchg
        mvi a,MM_SPS_SIZE
        call CDI_AHL

; set the AP readout size into the hardware
        lxi h,GBL_APH_NWRO+2
        mov d,M
        inx h
        mov e,M
        xchg
        mvi a,MM_AP_SIZE
        call CDI_AHL

        ret

; I_GENERIC_CMD 
; (zeros the time -- a test function)
generic_cmd:
        mvi a,0
        out SC_TIME_0_O ; (low) (write first)
        out SC_TIME_1_O 
        out SC_TIME_2_O 
        out SC_TIME_3_O 
        out SC_TIME_4_O 
        out SC_TIME_5_O  ; (high) (write last)
        jmp exitok

; I_TLM_FLUSH
tlm_flush: 
        mvi a,1
        sta GBL_FLUSH_REQUEST
        jmp exitok


; I_CAM_HV_ON EXPECTED=xxxxxxxxxxxx  (12)
cam_hv_on:
; compare HV settings in command with
; settings read from HV registers
        in PHOS_DAY_I
        cmp M
        jnz badhv
        inx h
        in PHOS_SPR_AUR_I
        cmp M
        jnz badhv
        inx h
        in MCP_DAY_I
        cmp M
        jnz badhv
        inx h
        in MCP_NITE_1_I
        cmp M
        jnz badhv
        inx h
        in MCP_NITE_2_I
        cmp M
        jnz badhv
        inx h
        in MCP_NITE_3_I
        cmp M
        jnz badhv
        inx h
        in MCP_NITE_4_I
        cmp M
        jnz badhv
        inx h
        in MCP_NITE_5_I
        cmp M
        jnz badhv
        inx h
        in MCP_NITE_6_I
        cmp M
        jnz badhv
        inx h
        in MCP_NITE_7_I
        cmp M
        jnz badhv
        inx h
        in PHOS_MAX_I
        cmp M
        jnz badhv
        inx h
        in MCP_MAX_I
        cmp M
        jnz badhv
; all Camera HV settings are as expected
; Turn on the power service
        mvi a,9         ; PS9
        call pwr_set_on
        jmp exitok

; I_SPA_HV_ON EXPECTED=xxxxxxxxx (9)
spa_hv_on:
; compare HV settings in command with
; settings read from HV registers
        in SP1_DAY_I
        cmp M
        jnz badhv
        inx h
        in SP1_AUR_I
        cmp M
        jnz badhv
        inx h
        in SP1_SPR_I
        cmp M
        jnz badhv
        inx h
        in SP2_DAY_I
        cmp M
        jnz badhv
        inx h
        in SP2_AUR_I
        cmp M
        jnz badhv
        inx h
        in SP2_SPR_I
        cmp M
        jnz badhv
        inx h
        in SP3_DAY_I
        cmp M
        jnz badhv
        inx h
        in SP3_AUR_I
        cmp M
        jnz badhv
        inx h
        in SP3_SPR_I
        cmp M
        jnz badhv
; all SPA HV settings are as expected
; Turn on the power service
        mvi a,10        ; PS10
        call pwr_set_on
        jmp exitok


; I_SPB_HV_ON EXPECTED=xxxxxxxxx (9)
spb_hv_on:
; compare HV settings in command with
; settings read from HV registers
        in SP4_DAY_I
        cmp M
        jnz badhv
        inx h
        in SP4_AUR_I
        cmp M
        jnz badhv
        inx h
        in SP4_SPR_I
        cmp M
        jnz badhv
        inx h
        in SP5_DAY_I
        cmp M
        jnz badhv
        inx h
        in SP5_AUR_I
        cmp M
        jnz badhv
        inx h
        in SP5_SPR_I
        cmp M
        jnz badhv
        inx h
        in SP6_DAY_I
        cmp M
        jnz badhv
        inx h
        in SP6_AUR_I
        cmp M
        jnz badhv
        inx h
        in SP6_SPR_I
        cmp M
        jnz badhv
; all SPB HV settings are as expected
; Turn on the power service
        mvi a,11        ; PS11
        call pwr_set_on
        jmp exitok

badhv:  mvi a,7  ; ERR_HV
        jmp exit