***************************************************************************
***************************************************************************
**   M E G A S Q U I R T - 2 0 0 1 - V1.01
**
**   B.A.Bowling And A.C.Grippo
**
***************************************************************************
***************************************************************************

; added continuous barometer updating in alpha-N mode.
;       NOTE: this means that the MAP pipe must be to open air when in
;       Alpha-N mode

; From Ver 2.0 of B&G code
; Fixed "sticky" decell bit
; Fixed Battery voltage correction roll-over problem.

; moved code start to $8080 to get-around page zero bug in bootloader
; alpha-N code included, switched by alpha_n bit in config13
; EGOrpm limit implemented
; throttle var uses throttlefactor.inc file for %age this can then be edited
; to represent the true %age of actual throttle travel, rather than the
; theoretical range of the adc
; NB / WB O2 sensors selectable via bit 2 in config13

; Version 1.01 for Inverted Injector driver MC33151:
; To change output from inverted to non-inverted, change all of
; the bset and bclr calls for inject1 and inject2 to their inverse.

$header 'MegaSquirt Embedded Code v.ef.1.00'
$pagewidth  130
$pagelength  90

$nolist
        include "gp32.equ"
$list

        org     ram_start
        include "megasquirt.h"

***************************************************************************
**
** Main Routine Here - Initialization and main loop
**
***************************************************************************
        org     {rom_start+128}
Start:

; All the 1.00 initialization code duplicated the bootloader code,
; see boot_r12.asm.

; Move the stack pointer out of the way
      ldhx  #init_stack+1               ; initialize
      txs                                 ;  the stack pointer

; Set up RAM Variables
        include "init.inc"

; Set up the port data-direction registers
        lda     #%00000000
        sta     ddrb                   ; Set as inputs (ADC will select which channel later)
        lda     #%00110000             ; Turn off injectors (inverted output)
        sta     portd
        lda     #%11110000
        sta     ddrd                   ; Outputs for injector
        clr     porta                  ; Turn off everything
        lda     #%11111111
        sta     ddra                   ; Outputs for fuel pump, fast idle and nitrous control
        lda     #$00
        sta     portc
        lda     #%00011111             ; ** Was 11111111
        sta     ddrc                   ; Outputs for LED
        lda     #%00000001             ; Serial Comm Port
        sta     ddre

; Set up the Real-time clock Timer (TIM2)
        MOV     #Timerstop,t2sc        ; Stop Timer so it can be set up
        mov     #$00,T2MODH
        mov     #$B8,T2MODL            ; set timer modulus register to 184 decimal
        mov     #T2SC0_No_PWM,T2SC0    ; make this normal port output (PWM MODE is #$5E)
        mov     #$00,T2CH0H
        mov     #$00,T2CH0L
        MOV     #Timergo,T2SC          ;  set timer prescaler to divide by 4

; Set up the PWM for the Injector (for current limit mode)
; PWM1 and PWM2 are separate in case of different types of injectors.
        MOV     #Timerstop,t1sc        ; Stop Timer so it can be set up
        mov     #$00,T1MODH
        mov     #100T,T1MODL           ; set timer modulus register

        mov     #T1SCX_NO_PWM,T1SC0    ; make this normal port output (PWM MODE is #$5E)
        mov     #$00,T1CH0H
        lda     INJPWM1
        sta     T1CH0L



        mov     #T1SCX_NO_PWM,T1SC1    ; make this normal port output (PWM MODE is #$5E)
        mov     #$00,T1CH1H
        lda     INJPWM2
        sta     T1CH1L

;       MOV     #Timergo_NO_INT,T1SC          ;  No interrupts for this

; Set up SCI port
        lda     #$12                    ; This is 9600 baud w/ the osc frequency selected
        sta     scbr
        bset    ensci,scc1              ; Enable SCI
        bset    RE,SCC2                 ; Enable receiver
        bset    SCRIE,SCC2              ; Enable Receive interrupt
        lda     SCS1                    ; Clear SCI transmitter Empty Bit

; Set up Interrupts
        mov     #%00000100,INTSCR       ;Enable IRQ

; Fire up the ADC, and perform one conversion to get the barometer value
        lda     #%01110000      ; Set up divide 8 and internal bus clock source
        sta     adclk
        lda     #%00000000      ; Select one conversion, no interrupt, AD0
        sta     adscr
        brclr   coco,adscr,*    ; wait until conversion is finished
        lda     adr
        sta     barometer       ; Store value in Barometer
        clr     adsel           ; Clear the channel selector

TURN_ON_INTS:
        cli                     ; Turn on all interrupts now


***************************************************************************
************************** M A I N  E V E N T  L O O P ********************
***************************************************************************
; All routines here are not time critical and happen asynchronously.

MainEventLoop:
        jsr     CalcRunningParameters
        jsr     SetIdle
        jsr     RPM_CALC
        jsr     CalcPWs
        jsr     EnableN2O
        jsr     CheckRevLimiter
     bra MainEventLoop

***************************************************************************
***************************************************************************




***************************************************************************
**
**  Correction Factor Lookup Table Access
**
**   Perform table lookup for barometer and air density correction factors,
**    and performs coolant temperature conversion from counts to degrees F
**
**   All tables are pre-computed for all 256 different values
**    and stored in FLASH
**
**   Note: Coolant temperature is in degrees F plus 40 - this allows
**    unsigned numbers for full temperature range
**
**   Also: Check the range of certain key variables and set to
**    default values of outside of range
**
***************************************************************************

CalcRunningParameters:
        clrh

        ldx     barometer
        lda     config13
        and     alpha_n
        beq     calc_run_sd           ; if SD
        ldx     map
calc_run_sd:
        lda     BAROFAC,x
        sta     barocor         ; Barometer Correction Gamma

        ldx     map
        lda     KPAFACTOR,x
        sta     kpa             ; Manifold Air Pressure in Kilopascals

        ldx     clt
        lda     THERMFACTOR,x
        sta     coolant         ; Coolant temperature in degrees F + 40

        ldx     mat
        lda     AIRDENFACTOR,x
        sta     AirCorr         ; Air Density Correction Factor

        ldx     tps
        lda     throttlefactor,x
        sta     throttle        ; Throttle position in percent.

        rts

LIMP_HOME:
; Perform some limit checks, put in some limp-home values and set codes?

;        lda     kpa
;        cmp     kparangeve
;        bhi     chk2

;        lda     kparangeve ; Limit value of Kpa to rail
;        sta     kpa

Limp_CHK2:
;        lda     clt
;        cmp     #$FF
;        blo     chk3
;        lda     #210T
;        sta     coolant

Limp_CHK3:
;        lda     mat
;        cmp     #$FF
;        blo     CHK4
;        lda     #100T
;        sta     AirCorr

Limp_CHK4:
; Anything else?

         rts

***************************************************************************
**
** Fast Idle Comparison - fast idle set is coolant below FAST IDLE value
**
***************************************************************************

SetIdle:
        lda     fastidle
        cmp     coolant
        bhi     SLOW_IDLE_SET
FAST_IDLE_SET:
        bclr    iasc,porta
        bra     SetIdle_DONE
SLOW_IDLE_SET:
        bset    iasc,porta

SetIdle_DONE:
        rts

***************************************************************************
**
** Computation of RPM
**
**   Result left in accumulator.
**
**   rpmk:rpmk+1
**   ----------- = rpm
**   rpmph:rpmpl
**
**  rpmk:rpmK+1 = RPM constant = 12,000/ncyl
**  rpmph:rpmpl = period count between IRQ pulsed lines, in 0.1 millisecond resolution
**
****************************************************************************

RPM_CALC:
        brclr     running,engine,RPM_CALC_DONE

        lda       rpmpl
        beq       RPM_CALC_DONE ; If zero then jump over calculation - prevent divide by zero
        lda       rpmph
        beq       FAST_RPM_CALC ; If we have only 8-bit denominator, then use native divide

SLOW_RPM_CALC:
        clr       intacc1
        clr       intacc1+1
        lda       rpmk
        sta       intacc1+2
        lda       rpmk+1
        sta       intacc1+3
        lda       rpmph
        sta       intacc2
        lda       rpmpl
        sta       intacc2+1

        jsr       udvd32        ; 32 x 32 divide

        lda       intacc1+3     ; get 8-bit RPM result
        sta       rpm
        bra       RPM_CALC_DONE

FAST_RPM_CALC:
        lda       rpmk
        psha
        pulh
        ldx       rpmpl
        lda       rpmk+1
        div
        sta       rpm

RPM_CALC_DONE:
        rts

***************************************************************************
** First, check RPM value to determine if we are cranking or running,
** then calculate the appropriate pulse width.
***************************************************************************
CalcPWs:
        lda     rpm
        cmp     crankrpm    ; Check if we are cranking, crankrpm RPM or lower

        bhi     RUN_IT
CRANK_IT:
        jsr     CRANKING_MODE
        bra     RPM_DONE
RUN_IT:
        jsr     CalcGammaE    ; Calcs GammaE
        jsr     Calc_PW1      ; Calcs PW for INJ 1
        jsr     Calc_PW2      ; Calcs PW for INJ 2

RPM_DONE:
        rts

Calc_PW1:                     ; Calcs PW for INJ 1
; set whatever is needed to indicate Bank 1
        jsr     VE_LOOKUP           ; calcs vecurr
        jsr     VE_CONTRIB          ; result in tmp1
        jsr     MAP_OVER_BARO       ; result in tmp1
        jsr     REQ_FUEL_EVAL       ; result in tmp11
        jsr     BATT_CORR_CALC      ; result in tmp6
        jsr     CALC_FINAL_PW1      ; result
        rts

Calc_PW2:                     ; Calcs PW for INJ 2
        lda     pwcalc1
        sta     pwcalc2
        rts


***************************************************************************
** Cranking Mode
**
** Pulsewidth is directly set by the coolant temperature value of
**  CWU (at -40 degrees) and CWH (at 165 degrees) - value is interpolated
**
***************************************************************************

CRANKING_MODE:
        bset    crank,engine
        bclr    startw,engine
        bclr    warmup,engine

        lda     throttle ; 80% comparison value for throttle - flood clear trigger
        cmp     #80T
        blo     INTERP_CRANK_PW

FLOOD_CLEAR:
        lda     #09    ; 0.9 ms pulsewidth - just opening...
        sta     pwcalc1
        bra     CRANKING_DONE

INTERP_CRANK_PW:
        lda     #$00
        sta     tmp1
        lda     #205T   ; 165 + 40 degrees (because of offset in lookup table)
        sta     tmp2
        mov     cwu,tmp3
        mov     cwh,tmp4
        mov     coolant,tmp5
        jsr     LinInterp
        mov     tmp6,pwcalc1

CRANKING_DONE:
        rts


***************************************************************************
**
** Computation of enrichments
**
**  The following equation is evaluated here:
**
**             TPSFuelCut   BaroCorr   AirCorr   EGOCorr
**    Warmup * ---------- * -------- * ------- * -------- = GammaE
**                100         100        100       100
**
**    Notice that the divide by 100 for Warmup is not done until later
**    to maintain precision.
**
***************************************************************************

CalcGammaE:
        jsr     WUE_CALC
        jsr     TAE_CALC
        jsr     EGO_CALC

WARMACCEL_COMP:
        ldx     warmcor
        lda     TPSfuelcut
        mul
        pshx
        pulh
        ldx     #100T
        div
        bcs     UPPER_RAIL
        psha
        pshh
        pula
        cmp     #50T
        ble     ND1
        pula
        inca
        bra     AirCorr_COMP
ND1:
        pula

AirCorr_COMP:
        tax
        lda     AirCorr
        mul
        pshx
        pulh
        ldx     #100T
        div
        bcs     UPPER_RAIL
        psha
        pshh
        pula
        cmp     #50T
        ble     ND2
        pula
        inca
        bra     EGOCORR_COMP
ND2:
        pula

EGOCORR_COMP:
        tax
        lda     EGOcorr
        mul
        pshx
        pulh
        ldx     #100T
        div
        bcs     UPPER_RAIL
        psha
        pshh
        pula
        cmp     #50T
        ble     ND3
        pula
        inca
        bra     BAROCOR_COMP
ND3:
        pula

BAROCOR_COMP:
        tax
        lda     barocor
        mul
        pshx
        pulh
        ldx     #100T
        div
        bcs     UPPER_RAIL
        psha
        pshh
        pula
        cmp     #50T
        ble     ND4
        pula
        inca
        bra     GammaE_SAVE
ND4:
        pula

GammaE_SAVE:
        sta     GammaE
        bra     GammaE_DONE

; You are here because somewhere along the way there was a overflow, so the best
;  enrichment that can be "guessed" is $FF
UPPER_RAIL:
        lda     #$F1    ;250
        sta     GammaE

GammaE_DONE:
        rts

***************************************************************************
**
** Contribution of VE
**
**  The following equation is evaluated here:
**
**   VE
**  ---- * GammaE = tmp1
**  100
**
**  Note that there is still a factor of 1/100 to be evaluated later.
***************************************************************************

VE_CONTRIB:
        ldx     GammaE
        lda     vecurr
        mul
        pshx
        pulh
        ldx     #100T
        div
        bcs     VE_RAIL
        psha
        pshh
        pula
        cmp     #50T
        ble     ND5
        pula
        inca
        bra     INTER1_SAVE
ND5:
        pula

INTER1_SAVE:
        sta     tmp1
        bra     VE_CONTRIB_DONE

VE_RAIL:
        lda     #$F2    ;251
        sta     tmp1

VE_CONTRIB_DONE:
      rts

***************************************************************************
**
** Calculation of MAP
**
**  The following equation is evaluated here:
**
**   MAP
**  ----- * tmp1 = tmp1
**   100
**
**  MAP is in direct units of KPa (everything is scaled at 100 KPa)
**
**  Note that there is still that factor of 1/100 still to be evaluated...
***************************************************************************
MAP_OVER_BARO:
        lda     config13
        and     alpha_n
        beq     SD_CALC  ; if SD then do MAP calc

ALPHA_N_CALC:
        bra     MAP_OVER_BARO_DONE

SD_CALC:
        ldx     tmp1
        lda     kpa
        mul
        pshx
        pulh
        ldx     #100T
        div
        bcs     MB_RAIL
        psha
        pshh
        lda     #50T
        sta     tmp2
        pula
        cmp     tmp2
        ble     ND6
        pula
        inca
        bra     INTER2_SAVE
ND6:
        pula

INTER2_SAVE:
        sta     tmp1
        bra     MAP_OVER_BARO_DONE

; To be here is really unlikely, unless this is a turbo (then use the correct MAP gauge!).
MB_RAIL:
        lda     #100T
        sta     tmp1
MAP_OVER_BARO_DONE:
      rts

***************************************************************************
**
** Calculation of Required Fuel Time
**
**  The following equation is evaluated here:
**
**   REQ_FUEL_K
**  ------------ * tmp1 = tmp11
**      100
**
**  Finally, the freakin factor of 100 is gone!
***************************************************************************
REQ_FUEL_EVAL:
        ldx     tmp1
        lda     REQ_FUEL
        mul
        pshx
        pulh
        ldx     #100T
        div
        bcs     RF_RAIL
        psha
        pshh
        pula
        cmp     #50T
        ble     ND7
        pula
        inca
        bra     INTER3_SAVE
ND7:
        pula

INTER3_SAVE:
        sta     tmp11
        bra     RF_DONE

; You are really screwed if you are here - you get the maximum pulsewidth for punishment!
RF_RAIL:
        lda     #$FE
        sta     tmp11

RF_DONE:
      rts


***************************************************************************
**
** Calculation of Final Pulse Width
**
**  The following equation is evaluated here:
**
**  pwcalc1 = tmp6 + TMP11 + TPSACCEL - INJOCFUEL
**
**  Note that InjOCFuel (injected fuel during injector open and
**  close) is currently a constant - eventually it will be a function
**  of battery voltage.
**
***************************************************************************
CALC_FINAL_PW1:
        lda     tmp11          ; From required fuel, above.
        add     tmp6
        bcs     MAX_PWM_ALLOWED1
        add     TPSACCEL
        bcs     MAX_PWM_ALLOWED1
;        sub     InjOCFuel
        sta     pwcalc1
        bra     FINISHED_PW_COMP

MAX_PWM_ALLOWED1:
        lda     #$FE
        sta     pwcalc1
        bra     FINISHED_PW_COMP

CALC_FINAL_PW2:
        lda     tmp11          ; From required fuel.
        add     tmp6          ; from batt correction
        bcs     MAX_PWM_ALLOWED1
        add     TPSACCEL
        bcs     MAX_PWM_ALLOWED2
;        sub     InjOCFuel
        sta     pwcalc1
        bra     FINISHED_PW_COMP

MAX_PWM_ALLOWED2:
        lda     #$FE
        sta     pwcalc2

FINISHED_PW_COMP:
        rts

***************************************************************************
**  PW Correction Factor subroutines.
***************************************************************************
***************************************************************************
**
** Warm-up and After-start Enrichment Section
**
** The Warm-up enrichment is a linear interpolated value from WWU (10 points)
**  which are placed at different temperatures
**
** Method:
**
** 1) Perform ordered table search of WWU (using coolant variable) to determine
**  which bin.
** 2) Perform linear interpolation to get interpolated warmup enrichment
**
** Also, the after-start enrichment value is calculated and applied here - it
**  is an added percent value on top of the warmup enrichment, and it is applied
**  for the number of ignition cycles specified in AWC. This enrichment starts
**  at a value of AWEV at first, then it linearly interpolates down to zero
**  after AWC cycles.
**
** 3) If (startw, engine is set) then:
** 4)  compare if (awc < ASEcount) then:
** 5)   x1=0, x2=AWC, y1=AWEV, y2=0, x=ASEcount, y=ASEenrichment
** 6)  else clear startw bit in engine
**
***************************************************************************
WUE_CALC:
        brclr   crank,engine,WUE1
        bclr    crank,engine
        bset    startw,engine
        bset    warmup,engine
        clr     ASEcount
WUE1:
        ldhx    #WWURANGE
        sthx    tmp1
        lda     #$09
        sta     tmp3
        lda     coolant
        sta     tmp4
        jsr     ORD_TABLE_FIND

        clrh
        ldx     tmp5
        lda     WWU,x
        sta     tmp4
        decx
        lda     WWU,x
        sta     tmp3
        mov     coolant,tmp5
        jsr     LinInterp
        mov     tmp6,warmcor
        lda     warmcor
        cmp     #100T
        bne     WUE2

; Outside of warmup range - clear warmup enrichment mode
        lda     #100T
        sta     warmcor
        bclr    startw,engine
        bclr    warmup,engine
        bclr    wled,portc
        bra     WUE_DONE

WUE2:
        bset    wled,portc
        brclr   startw,engine,WUE_DONE
        lda     ASEcount
        cmp     awc
        bhi     WUE3

        clr     tmp1
        mov     AWC,tmp2
        mov     AWEV,tmp3
        clr     tmp4
        mov     ASEcount,tmp5
        jsr     LinInterp
        lda     tmp6
        add     warmcor
        sta     warmcor
        bra     WUE_DONE

WUE3:
        bclr    startw,engine

WUE_DONE:
        rts

***************************************************************************
**
**  Throttle Position Acceleration Enrichment
**
**   Method is the following:
**
**
**   ACCELERATION ENRICHMENT:
**   If (TPS < last_TPS) goto DEACCELERATION_ENRICHMENT
**   If (TPS - last_TPS) > TPSthresh and TPSAEN = 0 then (acceleration enrichment):
**   {
**    1) Set accelration mode
**    2) Continuously determine rate-of-change of throttle, and perform
**        interpolation of TPSAQ values to determine acceleration
**        enrichment amount to apply.
**   }
**   If (TPSACLK > TPSACLKCMP) and TPSAEN is set then:
**   {
**    1) Clear TPSAEN bit in engine
**    2) Set TPSACCEL to 100%
**    3) Go to EGO Delta Step Check Section
**   }
**
**
**   DEACCELERATION ENRICHMENT:
**   If (last_TPS - TPS) > TPSthresh then (deacceleration fuel cut)
**   {
**    If (TPSAEN = 1) then:
**    {
**      1) TPSACCEL = 100 percent (no acceleration)
**      2) Clear TPSAEN bit in ENGINE
**      3) Go to EGO Delta Step
**    }
**    If (RPM > 15 (corresponding to 1500 RPM)) then (fuel cut mode):
**    {
**      1) Set TPSACCEL value to TPSDQ
**      2) Set TPSDEN bit in ENGINE
**      3) Go to EGO Delta Step Check Section
**    }
**   }
**   else
**   {
**    If (TPSDEN = 1) then
**    {
**     1) Clear TPSDEN bit in ENGINE
**     2) TPSACCEL = 100%
**     3) Go to EGO Delta Step Check Section
**    }
**   }
**
***************************************************************************
TAE_CALC:
        sei
        mov     TPS,tmp1
        mov     last_TPS,tmp2
        cli
        lda     tmp1
        cmp     tmp2
        blo     TDE

AE_CHK:
        lda     tmp1
        sub     tmp2
        cmp     TPSthresh
        blo     TAE_CHK_TIME
        brset   TPSAEN,ENGINE,AE_COMP_SHOOT_AMT

; Add in accel enrichment
        lda     TPSAQ           ; start out using first element - will determine actual next time around
        sta     TPSACCEL        ; Acceleration percent amount - used in later calculations
        clr     TPSACLK
        lda     TPSASYNC
        sta     TPSaclkcmp      ; Shoot time comparison value
        bset    TPSAEN,ENGINE
        bclr    TPSDEN,ENGINE
        bset    aled,portc
        bclr    wled,portc
        jmp     TAE_DONE


; First, calculate cold temperature add-on enrichment value from coolant value,
; from -40 degrees to 165 degrees.
;
; Then, calculate shoot amount (quantity) for acceleration enrichment
; Find bins (between) for corresponding TPSDOT, and linear interpolate
; to find enrichment amount (from TPSAQ). This is continuously
; checked every time thru main loop while in acceleration mode,
; and the highest value is latched and used.

AE_COMP_SHOOT_AMT:
        ; First, the amount based on cold temperatures
        clr     tmp1            ; 0 -> - 40 degrees
        mov     #205T,tmp2      ; 165 + 40 degrees (because of offset in lookup table)
        mov     TPSACOLD,tmp3   ; This is the amount at coldest
        clr     tmp4            ; no enrichment addon at warm temperature
        mov     coolant,tmp5
        jsr     LinInterp
        mov     tmp6,tmp13      ; result - save here temporarily

        ; Now the amount based on TPSDOT
        ldhx    #TPSdotrate
        sthx    tmp1
        mov     #$03,tmp3
        lda     TPS
        sub     last_TPS
        sta     tmp4    ; TPSDOT
        sta     tmp10   ; Save away for later use below
        jsr     ord_table_find

        clrh
        ldx     tmp5
        lda     TPSAQ,x
        sta     tmp4
        decx
        lda     TPSAQ,x
        sta     tmp3
        mov     tmp10,tmp5
        jsr     LinInterp
        lda     tmp6
        add     tmp13           ; Add on the amount computed in cold temperature enrich above
        sta     tmp6
        cmp     TPSACCEL
        blo     TAE_CHK_TIME
        lda     tmp6            ; Replace with this higher value
        sta     TPSACCEL

; Check if acceleration done
TAE_CHK_TIME:
;        brset   TPSDEN,ENGINE,RST_ACCEL
        lda     TPSaclk
        cmp     TPSaclkcmp
        blo     TAE_DONE
        bclr    TPSAEN,ENGINE
        mov     #100T,TPSFuelCut
        clr     TPSACCEL
        bclr    aled,portc
        bra     TAE_DONE

; deaccel
TDE:
        lda     tmp2
        sub     tmp1

        cmp     TPSthresh
        blo     TDE_CHK_DONE
        brclr   TPSAEN,ENGINE,TDE_CHK_FUEL_CUT
        mov     #100T,TPSFuelCut
        clr     TPSACCEL
        bclr    TPSAEN,ENGINE
        bclr    aled,portc
        bra     TAE_DONE

TDE_CHK_FUEL_CUT:
        lda     rpm
        cmp     #$0F
        blo     TAE_DONE
        lda     TPSDQ
        sta     TPSFUELCUT
        bset    TPSDEN,ENGINE
        bclr    TPSAEN,ENGINE
        bclr    aled,portc
        bra     TAE_DONE

TDE_CHK_DONE:
        brclr   TPSDEN,ENGINE,TAE_DONE
        bclr    TPSDEN,ENGINE
        lda     #100T
        sta     TPSFUELCUT
        clr     TPSACCEL

TAE_DONE:
        rts

***************************************************************************
**
**  Exhaust Gas Oxygen Sensor Measurement Section
**
**   Steps are the following:
**
**   If EGOdelta = 0                                 then skipo2
**   If RPM < ego_rpm                                then skipo2
**   If TPSAEN in ENGINE or TPSDEN in ENGINE are set then skipo2
**   If coolant < EGOtemp                            then skipo2
**   If sech = 0 and secl < 30 seconds               then skipo2 (skip first 30 seconds)
**   If TPS > 3.5 volts                              then skipo2
**
**   If EGOcount > EGOcountcmp {
**      EGOcount = 0
**      If EGO > 26 (counts, or 0.5 Volts) then (rich) {
**         tpm = EGOcurr - EGOdelta
**         if tpm >= EGOlimit then EGOcorr = tpm
**         return
**      }
**      else (lean) {
**         tpm = EGOcorr + EGOdelta
**         if tpm > EGOlimit then EGOcorr = tpm
**         return
**      }
**   }
**
**   skipo2:
**   EGOcorr = 100%
**
***************************************************************************
EGO_CALC:
        lda     EGOdelta
        beq     SKIPO2
        lda     rpm
        cmp     EGOrpm
        blo     SKIPO2
        brset   TPSAEN,ENGINE,SKIPO2
        brset   TPSDEN,ENGINE,SKIPO2
        lda     coolant
        cmp     EGOtemp
        blo     SKIPO2
        lda     TPS
        cmp     #$B2
        bhi     SKIPO2
        lda     sech
        bne     chk_o2_lag      ; if high seconds set then we can check o2
        lda     secl
        cmp     #30T    ; 30 seconds threshold
        blo     SKIPO2

; Check if exceeded lag time - if so then we can modify EGOcorr
CHK_O2_LAG:
        lda     EGOcount
        cmp     EGOcountcmp
        blo     EGO_DONE

; Check if rich/lean
        clr     egocount

        lda     config13        ; Check Lambda type (0='standard' NB type)
        and     Lambda
        beq     NB_Sensor
WB_Sensor:                      ; lower V is rich
        lda     ego
        cmp     O2TargetV
        blo     O2_IS_RICH
        bra     O2_IS_LEAN

NB_Sensor:                      ; higher V is rich
        lda     ego
        cmp     O2TargetV
        blo     O2_IS_LEAN

; rich o2 - lean out egocorr
O2_IS_RICH:
        lda     #100T
        sub     EGOlimit    ; Generate the lower limit rail point
        sta     tmp2
        lda     EGOcorr
        sub     EGOdelta
        sta     tmp1
        cmp     tmp2
        blo     EGO_DONE    ; railed at EGOlimit value
        lda     tmp1
        sta     EGOcorr
        bra     EGO_DONE

; lean o2 - richen EGOcorr
O2_IS_LEAN:
        lda     #100T
        add     EGOlimit    ; Generate the upper limit rail point
        sta     tmp2

        lda     EGOcorr
        add     EGOdelta
        sta     tmp1
        cmp     tmp2
        bhi     EGO_DONE    ; railed at EGOlimit value
        lda     tmp1
        sta     EGOcorr
        bra     EGO_DONE

; reset EGOcorr to 100%
SKIPO2:
        lda     #100T
        sta     EGOcorr
EGO_DONE:
        rts

***************************************************************************
**
**  VE 3-D Table Lookup
**
**   This is used to determine value of VE based on RPM and MAP
**   The table looks like:
**
**      105 +....+....+....+....+....+....+....+
**          ....................................
**      100 +....+....+....+....+....+....+....+
**                     ...
**   KPA                 ...
**                         ...
**       35 +....+....+....+....+....+....+....+
**          5    15   25   35   45   55   65   75 RPM/100
**
**
**  Steps:
**   1) Find the bracketing KPA positions via ORD_TABLE_FIND, put index in tmp8 and
**       bounding values in tmp9(kpa1) and tmp10(kpa2)
**   2) Find the bracketing RPM positions via ORD_TABLE_FIND, store index in tmp11 and
**       bounding values in tmp13(rpm1) and tmp14(rpm2)
**   3) Using the VE table, find the table VE values for tmp15=VE(kpa1,rpm1),
**       tmp16=VE(kpa1,rpm2), tmp17 = VE(kpa2,rpm1), and tmp18 = VE(kpa2,rpm2)
**   4) Find the interpolated VE value at the lower KPA range :
**       x1=rpm1, x2=rpm2, y1=VE(kpa1,rpm1), y2=VE(kpa1,rpm2) - put in tmp19
**   5) Find the interpolated VE value at the upper KPA range :
**       x1=rpm1, x2=rpm2, y1=VE(kpa2,rpm1), y2=VE(kpa2,rpm2) - put in tmp11
**   6) Find the final VE value using the two interpolated VE values:
**       x1=kpa1, x2=kpa2, y1=VE_FROM_STEP_4, y2=VE_FROM_STEP_5
**
***************************************************************************
VE_LOOKUP:
        lda     tps
        sta     load
        lda     config13
        and     alpha_n
        bne     VE_STEP_1           ; if alpha-N
        lda     kpa
        sta     load

VE_STEP_1:
        ldhx    #KPARANGEVE_f
        sthx    tmp1
        lda     #$07
        sta     tmp3
        lda     load
        sta     tmp4
        jsr     ORD_TABLE_FIND
        lda     tmp1
        lda     tmp2
        mov     tmp5,tmp8    ; Index
        mov     tmp1,tmp9    ; X1
        mov     tmp2,tmp10   ; X2

VE_STEP_2:
        ldhx    #RPMRANGEVE_f
        sthx    tmp1
        lda     #$07
        sta     tmp3
        lda     rpm
        sta     tmp4
        jsr     ORD_TABLE_FIND
        mov     tmp5,tmp11     ; Index
        mov     tmp1,tmp13     ; X1
        mov     tmp2,tmp14     ; X2

VE_STEP_3:
        clrh
        lda     #$08
        psha
        pulx
        lda     tmp8
        deca
        mul
        add     tmp11
        deca
        tax
        lda     VE,x
        sta     tmp15
        incx
        lda     VE,x
        sta     tmp16
        lda     #$08
        psha
        pulx
        lda     tmp8
        mul
        add     tmp11
        deca
        tax
        lda     VE,x
        sta     tmp17
        incx
        lda     VE,x
        sta     tmp18
        jmp     VE_STEP_4

VE_STEP_4:
        mov     tmp13,tmp1
        mov     tmp14,tmp2
        mov     tmp15,tmp3
        mov     tmp16,tmp4
        mov     rpm,tmp5
        jsr     LinInterp
        mov     tmp6,tmp19

VE_STEP_5:
        mov     tmp13,tmp1
        mov     tmp14,tmp2
        mov     tmp17,tmp3
        mov     tmp18,tmp4
        mov     rpm,tmp5
        jsr     LinInterp
        mov     tmp6,tmp11

VE_STEP_6:
        mov     tmp9,tmp1
        mov     tmp10,tmp2
        mov     tmp19,tmp3
        mov     tmp11,tmp4
        mov     load,tmp5
        lda     load
        jsr     LinInterp
        mov     tmp6,vecurr

VE_DONE:
        rts

***************************************************************************
**
** Calculation of Battery Voltage Correction for Injector Opening Time
**
** Leaves result in liY.
** Mangles tmp1-tmp5.
**
** Injector open time is implemented as a linear function of
**  battery voltage, from 7.2 volts (61 ADC counts) to 19.2 volts (164 counts),
**  with 13.2 volts (113 counts) being the nominal operating voltage
**
** INJOPEN = injector open time at 13.2 volts in mms
** BATTFAC = injector open adjustment factor 6 volts from 13.2V in mms
**
**
** + (INJOPEN + BATTFAC)
** +   *
** +                     (INJOPEN)
** +                         *
** +                                       (INJOPEN - BATTFAC)
** +                                               *
** +
** ++++++++++++++++++++++++++++++++++++++++++++++++++++++
**    7.2V                 13.2V                19.2
**
***************************************************************************

BATT_CORR_CALC:
        lda     #61T
        sta     tmp1
        lda     #164T
        sta     tmp2
        lda     injopen
        add     battfac
        sta     tmp3
        lda     injopen
        sub     battfac
        sta     tmp4
        bpl     MBFF    ; Check if minus comdition
        clr     tmp4
MBFF:
        mov     batt,tmp5
        jsr     lininterp  ; injector open time in tmp6
        rts

***************************************************************************
**  Nitrous control - assumes a driver like the fast idle circuit is
**  connected to pin 35.
**
**  Turn on the N2O solenoids when the following are all true:
**     cts >= minN2OTemp
**     rpm >= lowN2ORPM
**     rpm <  highN2ORPM (make sure to set high limit below revLimit)
**     TPS >= N2OOnTPS
**
**  It would be nice if we had another adc hooked to a fuel pressure
**  transducer, too.  Get around this by putting pressure switches on
**  the inputs of both solenoids, so we know we have enough fuel and N2O.
***************************************************************************

EnableN2O:
        lda     coolant
        cmp     minN2Otemp
        blo     N2Ooff
        lda     rpm
        cmp     lowN2ORPM
        blo     N2Ooff
        cmp     highN2ORPM
        bge     N2Ooff

        lda     throttle
        cmp     #95T    ; Must have pedal substantially mashed.
; for the previous line to work the throttlefactor file needs to be setup
; properly for the actual installation.
        blo     N2Ooff

N2Oon:
        bset    N2O,porta
        bra     N2Odone

N2Ooff:
        bclr    N2O,porta

N2Odone:
        rts

***************************************************************************
**  Check rev limit value and perform a hard cutoff.
***************************************************************************
;***** commented out at the moment as you have no way of setting/checking
; rev limit using the PC
CheckRevLimiter:
;        lda     rpm
;        cmp     revLimit
;        ble     CheckRevsOk
;        lda     #1T      ; MinPulseWidth
;        sta     pwcalc1
;        sta     pwcalc2
CheckRevsOk:
        rts

***************************************************************************
**                     * * * * Interrupt Section * * * * *
**
**  Following interrupt service routines:
**  - Timer Overflow
**  - ADC Conversion Complete
**  - IRQ input line transistion from high to low
**  - Serial Communication received character
**  - Serial Communications transmit buffer empty (send another character)
**
***************************************************************************
**
**  Timer Rollover - Occurs every 1/10 of a millisecond - main timing clock
**
**     Generate time rates:
**        1/10 milliseconds
**        1 milliseconds
**        1/10 seconds
**        seconds
**
**  Also, in 1/10 millisecond section, turn on/off injector and
**  check RPM for stall condition.
**
**  In milliseconds section, fire off ADC conversion for next channel
**  (5 total), and wrap back when all channels done.
**
***************************************************************************

TIMERROLL:
;==========================================================================
;***************** 0.1 millesecond section ********************************
;==========================================================================
        inc      mms            ; bump up 0.1 millisec variable

;-------------------------------------------------------------------------------
;======== Injector Firing Control ========
;===== Main Injector Control Logic =======

        brset    sched1,squirt,NEW_SQUIRT1
InjF1   brset    sched2,squirt,NEW_SQUIRT2
InjF2   brset    firing1,squirt,CHK_DONE_1
InjF3   brset    firing2,squirt,CHK_DONE_2
        jmp      CHECK_RPM

;-------------------------------------------------------------------------------
;=== Injector #1 - Start New Injection ===

NEW_SQUIRT1:
        bset     firing1,squirt  ; Turn on "firing" bit
        bclr     sched1,squirt   ; Turn off schedule bit (is now current operation)
        bset     inj1,squirt
        bset     sled,portc      ; squirt LED is ON

        mov      #$00,T1CH0H
        lda      INJPWM1
        sta      T1CH0L
        bclr     inject1,portd   ;^* * * Turn on Injector #1 (inverted drive)
        bra      InjF1

;-------------------------------------------------------------------------------
;=== Injector #2 - Start New Injection ===

NEW_SQUIRT2:
        bset     firing2,squirt  ; Turn on "firing" bit
        bclr     sched2,squirt   ; Turn off schedule bit (is now current operation)
        bset     inj2,squirt
        bset     sled,portc      ; squirt LED is ON

        mov      #$00,T1CH1H
        lda      INJPWM2
        sta      T1CH1L
        bclr     inject2,portd   ;^* * * Turn on Injector #1 (inverted drive)
        bra      InjF2

;-------------------------------------------------------------------------------
;=== Injector #1 - Check for end of Injection ===

CHK_DONE_1:
        inc      pwrun1
        lda      pwrun1
        cmp      pw1
        beq      OFF_INJ_1 ; Pulse is over
        lda      pwrun1
        cmp      INJPWMT1  ; Full current is done, go to PWM phase of pulse.
        beq      PWM_LIMIT_1
        bra      InjF3

OFF_INJ_1:
        bclr     firing1,squirt
        bclr     sched1,squirt
        bclr     inj1,squirt
        bset     inject1,portd  ;^* * * Turn Off Injector #1 (inverted drive)
        mov      #Timerstop,T1SC
        mov      #t1scx_NO_PWM,T1SC0
        mov      #Timergo_NO_INT,T1SC
        bra      InjF3

PWM_LIMIT_1:
        mov      #Timerstop,T1SC
        mov      #T1SCX_PWM,T1SC0 ; Set pulse duty cycle to user-specified value.
        mov      #Timergo_NO_INT,T1SC
        bra      InjF3

;-------------------------------------------------------------------------------
;=== Injector #2 - Check for end of Injection ===

CHK_DONE_2:
        inc      pwrun2
        lda      pwrun2
        cmp      pw2
        beq      OFF_INJ_2
        lda      pwrun2
        cmp      INJPWMT2
        beq      PWM_LIMIT_2
        bra      CHECK_RPM

OFF_INJ_2:
        bclr     firing2,squirt
        bclr     sched2,squirt
        bclr     inj2,squirt
        bset     inject2,portd  ;^* * * Turn Off Injector #1 (inverted drive)
        mov      #Timerstop,T1SC
        mov      #t1scx_NO_PWM,T1SC1
        mov      #Timergo_NO_INT,T1SC
        bra      CHECK_RPM

PWM_LIMIT_2:
        mov      #Timerstop,T1SC
        mov      #T1SCX_PWM,T1SC1
        mov      #Timergo_NO_INT,T1SC
        bra      CHECK_RPM

;-------------------------------------------------------------------------------
;--  Check RPM Section

CHECK_RPM:
        brclr    running,engine,ENABLE_THE_IRQ ; Branch if not running right now
        brset    firing1,squirt,CHK_RE_ENABLE
        brset    firing2,squirt,CHK_RE_ENABLE
        bclr     sled,portc     ; squirt LED is OFF - nothing is injecting

;====== Check for re-enabling of IRQ input pulses when half of the previous RPM period has elapsed.
CHK_RE_ENABLE:
        lda      rpmpl          ; Load in the latched previous RPM value
        lsra                    ; divide by two
        cmp      rpmcl          ; Is it the same value as current RPM Counter?
        bne      INCRPMER       ; If not then jump around this
        bset     ACK,INTSCR     ; clear out any latched interrupts
        bclr     IMASK,INTSCR   ; enable interrupts again for IRQ

INCRPMER:
        inc      rpmcl          ; Increment RPM count low
        bne      CHECK_MMS      ; Didn't roll
        inc      rpmch
        lda      rpmch
        cmp      #100T          ; If RPMPH is 100 (or RPMPeriod=2.56 sec), then engine stalled
        bne      CHECK_MMS

        clr      engine         ; Engine is stalled, clear all in engine settings
        bclr     fuelp,porta    ; Turn off fuel pump
        bclr     Idle,porta     ; Turn off fast idle solenoid
        bclr     N2O,porta      ; Turn off nitrous
        clr      rpmch
        clr      rpmcl
        bclr     sled,portc     ; squirt LED is off
        bclr     wled,portc     ; warmup LED is off
        clr      pw1            ; zero out pulsewidths
        clr      pw2
        clr      rpm
        bclr     IMASK,INTSCR   ; enable all IRQ interrupts
        bra      CHECK_MMS

ENABLE_THE_IRQ:
        bclr     IMASK,INTSCR   ; Enable IRQ

CHECK_MMS:
        lda      mms
        cmp      #10T
        bne      RTC_DONE

;==========================================================================
;********************* millesecond section ********************************
;==========================================================================
MSEC:
        inc      ms             ; bump up millisec
        clr      mms

; Fire off another ADC conversion, channel is pointed to by ADSEL
        lda     adsel
        ora     #%01000000
        sta     adscr

MSDONE:
        lda      ms
        cmp      #100T
        bne      RTC_DONE

;==========================================================================
;******************** 1/10 second section *********************************
;==========================================================================

ONETENTH:
        inc      tenth
        inc      TPSaclk
        clr      ms

; Save current TPS reading in last_TPS variable to compute TPSDOT in acceleration
;  enrichment section

        lda      TPS
        sta      last_TPS

        lda      tenth
        cmp      #10T
        bne      RTC_DONE

;==========================================================================
;********************** seconds section ***********************************
;==========================================================================

SECONDS:

        clr      tenth
        inc      secl           ; bump up second count
        bne      RTC_DONE
        inc      sech

RTC_DONE:
        lda      T2SC
        bclr     7,T2SC
        rti


***************************************************************************
**
** IRQ - Input trigger for new pulse event
**
** This line is connected to the input trigger (i.e TACH signal from ignition
**  system), and schedules a new injector shot (injector actually controlled in
**  0.1 timer section above)
**
***************************************************************************

DoSquirt:
        inc       ASEcount       ; Increment after-start enrichment counter
        inc       EGOcount       ; Increment EGO step counter

        lda       rpmch
        sta       rpmph
        lda       rpmcl
        sta       rpmpl
        clr       rpmch
        clr       rpmcl
        bset      fuelp,porta    ; Turn on fuel Pump
        bset      running,engine ; Set engine running value

        brset     crank,engine,SCHED_SQUIRT     ;Squirt on every pulse if cranking

        inc       IgnCount       ; Check to see if we are to squirt or skip
        lda       IgnCount
        cmp       divider
        beq       SCHED_SQUIRT
        lda       IgnCount
        cmp       #$08           ; The maximum allowed - reset if match
        bne       IRQ_EXIT
        clr       IgnCount
        bra       IRQ_EXIT

SCHED_SQUIRT:
        bset      fuelp,porta    ; Turn on fuel Pump
        bset      running,engine ; Set engine running value
        clr       IgnCount

        brset     crank,engine,SCHED_BOTH
        lda       alternate
        beq       SCHED_BOTH
        inc       altcount
        brset     0,altcount,SCHED_INJ2

;$MACRO Schedule
;        ; This macro crashes the assembler
;        mov       pwcalc%1,pw%1  ; Set counter limit
;       clr       pwrun%1        ; Clear running counter
;       bset      sched%1,squirt
;       bset      inj%1,squirt
;$MACROEND

SCHED_INJ1:
;       Schedule 1
        mov       pwcalc1,pw1   ; Latch calculated pulsewidth
        clr       pwrun1        ; Clear running counter
        bset      sched1,squirt
        bset      inj1,squirt
        bra       IRQ_EXIT

SCHED_INJ2:
;       Schedule  2
        mov       pwcalc2,pw2
        clr       pwrun2        ; Clear running counter
        bset      sched2,squirt
        bset      inj2,squirt
        bra       IRQ_EXIT

SCHED_BOTH:
 ;      Schedule  1
        mov       pwcalc1,pw1
        clr       pwrun1        ; Clear running counter
        bset      sched1,squirt
        bset      inj1,squirt
 ;      Schedule  2
        mov       pwcalc2,pw2
        clr       pwrun2        ; Clear running counter
        bset      sched2,squirt
        bset      inj2,squirt

IRQ_EXIT:
        bset      ACK,INTSCR     ; Flush out any new interrupts pending
        bset      IMASK,INTSCR   ; Disable IRQ interrupts to debounce ignition signal
        bset      IRQF,INTSCR
        rti


***************************************************************************
**
** ADC - Interrupt for ADC conversion complete
**
***************************************************************************
DoADC:
        pshh            ; Do this because processor does not stack H
        clrh

; Store previous values for derivatives
        ldx     adsel
        lda     map,x
        sta     lmap,x  ; Store the old value
        lda     adr
       ; Load in the new ADC reading

        adc     map,x   ; Perform (map + last_map)/2 averaging (for all ADC readings)
        rora
        sta     map,x   ; MAP is entry point, offset is loaded in index register

        lda     adsel
        inca
        cmp     #$06
        bne     ADCPTR
        clra
ADCPTR:
        sta     adsel

        pulh
        rti


***************************************************************************
**
** SCI Communications
**
** Communications is established when the PC communications program sends
** a command character - the particular character sets the mode:
**
** "A" = send all of the realtime variables via txport.
**
** "B" = jump to flash burner routine and burn VE/constant values in RAM into flash
**
** "C" = Test communications - echo back one character
**
** "S" = Signature echo, to verify that configurator is correct for
**       this version of the code.
**
** "V" = send the VE table and constants via txport (128 bytes)
**
** "W"+<offset>+<newbyte> = receive new VE or constant byte value and
**       store in offset location.
**
***************************************************************************
$MACRO Goto_Mode
        cmp     #'%1'
        beq     MODE_%1
$MACROEND

amode   equ     1
bmode   equ     0
cmode   equ     amode
smode   equ     2
vmode   equ     3
wmodeo  equ     4
wmodev  equ     wmodeo+1

SCI_RD:
        pshh
        lda     SCS1    ; Clear the SCRF bit by reading this register

        lda     txmode  ; Check if we are in the middle of a receive new VE/constant
        cmp     #wmodeo
        beq     MODE_W_O
        cmp     #wmodev
        beq     MODE_W_V

        lda     SCDR    ; Get the command byte
        Goto_Mode A
        Goto_Mode B
        Goto_Mode C
        Goto_Mode R
        Goto_Mode S
        Goto_Mode V
        Goto_Mode W
        bra     DONE_RCV

MODE_A: ; Send back all real-time variables
        clr     txcnt
        lda     #amode
        sta     txmode
        lda     #23T                    ; First 22 bytes in RAM
        sta     txgoal
        bset    TE,SCC2                 ; Enable Transmit
        bset    SCTIE,SCC2              ; Enable transmit interrupt
        bra     DONE_RCV

MODE_B: ; Burn RAM constants back into flash
        jsr     burnConst
        lda     bmode
        sta     txmode
        bra     DONE_RCV

MODE_C: ; Comm test
        clr     txcnt                   ; Just send back SECL variable to test comm port.
        lda     #cmode
        sta     txmode
        lda     #2T                     ; Just one byte
        sta     txgoal
        bset    TE,SCC2                 ; Enable Transmit
        bset    SCTIE,SCC2              ; Enable transmit interrupt
        bra     DONE_RCV

MODE_S: ; Send signature, 4 bytes of version information.
        clr     txcnt
        lda     #smode
        sta     txmode
        lda     Configuration           ; Grab length byte
        sta     txgoal
        bset    TE,SCC2                 ; Enable Transmit
        bset    SCTIE,SCC2              ; Enable transmit interrupt
        bra     DONE_RCV

MODE_V: ; Send VE & constants
        clr     txcnt
        lda     #vmode
        sta     txmode
        lda     #$7E
        sta     txgoal
        bset    TE,SCC2                 ; Enable Transmit
        bset    SCTIE,SCC2              ; Enable transmit interrupt
        bra     DONE_RCV

MODE_W: ; Write to RAM
        lda     #wmodeo
        sta     txmode
        bra     DONE_RCV

MODE_W_O:
        mov     SCDR,rxoffset
        inc     txmode
        bra     DONE_RCV

MODE_W_V:
        clrh
        ldx     rxoffset
        lda     SCDR
        sta     VE,x
        clr     txmode
        bra     DONE_RCV
MODE_R: ; Send back first 255 bytes of RAM
        clr     txcnt
        lda     #amode
        sta     txmode
        lda     #254T                    ; First 255 bytes in RAM
        sta     txgoal
        bset    TE,SCC2                 ; Enable Transmit
        bset    SCTIE,SCC2              ; Enable transmit interrupt
        bra     DONE_RCV

DONE_RCV:
        pulh
        rti


*** Transmit Character Interrupt Handler ***************
SCI_TD:
        pshh
        lda     SCS1    ; Clear the SCRF bit by reading this register
        clrh
        ldx     txcnt
        lda     txmode

A_C_MODE_XFER:
        cmp     #amode
        bne     V_MODE_XFER
        lda     secl,X
        bra     DO_XFER

S_MODE_XFER:
        cmp     #smode
        bne     V_MODE_XFER
        lda     Configuration+1,X
        bra     DO_XFER

V_MODE_XFER:
        cmp     #vmode
        bne     BAD_MODE_XFER
        lda     ve,x
        bra     DO_XFER

BAD_MODE_XFER:
        bra     XFER_CLEANUP

DO_XFER:
        sta     SCDR
        lda     txcnt
        inca
        sta     txcnt
        cmp     txgoal
        bne     DONE_XFER

XFER_CLEANUP:
        clr     txcnt
        clr     txgoal
        clr     txmode

        bclr    TE,SCC2                 ; Disable Transmit
        bclr    SCTIE,SCC2              ; Disable transmit interrupt

DONE_XFER:
        pulh
        rti

***************************************************************************
**
** Dummy ISR - just performs RTS
**
***************************************************************************
Dummy:                  ; Dummy vector - there just to keep the assembler happy
        rti

***************************************************************************
**
** Various functions/subroutines Follow
**
**  - Ordered Table Search
**  - Linear Interpolation
**  - 32 x 16 divide
***************************************************************************


***************************************************************************
**
** Ordered Table Search
**
**  X is pointing to the start of the first value in the table
**  tmp1:2 - initially hold the start of table address, then they hold
**         the bound values
**  tmp3 - the end of the table (nelements - 1)
**  tmp4 - the comparison value
**  tmp5 - the index result - if zero then comp value is less than
**          beginning of table, and if equal to nelements then it is
**          railed at upper end
**
***************************************************************************
ORD_TABLE_FIND:
        clr     tmp5
        ldhx    tmp1
        lda     ,x
        sta     tmp1
        sta     tmp2
;       cmp     tmp4
;       bhi     GOT_ORD_NUM

REENT:
        incx
        inc     tmp5
        mov     tmp2,tmp1
        lda     ,x
        sta     tmp2

        cmp     tmp4
        bhi     GOT_ORD_NUM
        lda     tmp5
        cmp     tmp3
        bne     REENT

;       inc     tmp5
;       mov     tmp2,tmp1

GOT_ORD_NUM:
        rts

***************************************************************************
**
** Linear Interpolation - 2D
** Does not extrapolate, if x is out of range, then corresponding Y
** end point is returned.
**
**            (y2 - y1)
**  Y = Y1 +  --------- * (x - x1)
**            (x2 - x1)
**
***************************************************************************

; Argument list
liX1      equ  tmp1
liX2      equ  tmp2
liY1      equ  tmp3
liY2      equ  tmp4
liX       equ  tmp5
liY       equ  tmp6

; Locals
negSlope  equ  tmp7

LinInterp:

        clr     negSlope    ; This is the negative slope detection bit
        mov     liY1,liY
CHECK_LESS_THAN:
        lda     liX
        cmp     liX1
        bhi     CHECK_GREATER_THAN
        bra     DONE_WITH_INTERP
CHECK_GREATER_THAN:
        lda     liX
        cmp     liX2
        blo     DO_INTERP
        mov     liY2,liY
        bra     DONE_WITH_INTERP

DO_INTERP:
        mov     liY1,liY
        lda     liX2
        sub     liX1
        beq     DONE_WITH_INTERP
        psha
        lda     liY2
        sub     liY1
        tsta
        bge     POSINTERP
        nega
        inc     negSlope
POSINTERP:
        psha
        lda     liX
        sub     liX1
        beq     ZERO_SLOPE
        pulx
        mul
        pshx
        pulh
        pulx

        div

        psha
        lda     negSlope
        bne     NEG_SLOPE
        pula
        add     liY1
        sta     liY
        bra     DONE_WITH_INTERP
NEG_SLOPE:
        pula
        sta     negSlope
        lda     liY1
        sub     negSlope
        sta     liY
        bra     DONE_WITH_INTERP
ZERO_SLOPE:
        pula            ;clean stack
        pula            ;clean stack
DONE_WITH_INTERP:
        rts



********************************************************************************
********************************************************************************
*
*     32 x 16 Unsigned Divide
*
*     This routine takes the 32-bit dividend stored in INTACC1.....INTACC1+3
*     and divides it by the 16-bit divisor stored in INTACC2:INTACC2+1.
*     The quotient replaces the dividend and the remainder replaces the divisor.
*
UDVD32    EQU     *
*
DIVIDEND  EQU     INTACC1+2
DIVISOR   EQU     INTACC2
QUOTIENT  EQU     INTACC1
REMAINDER EQU     INTACC1
*
        PSHH                            ;save h-reg value
        PSHA                            ;save accumulator
        PSHX                            ;save x-reg value
        AIS     #-3                     ;reserve three bytes of temp storage
        LDA     #!32                    ;
        STA     3,SP                    ;loop counter for number of shifts
        LDA     DIVISOR                 ;get divisor msb
        STA     1,SP                    ;put divisor msb in working storage
        LDA     DIVISOR+1               ;get divisor lsb
        STA     2,SP                    ;put divisor lsb in working storage
*
*     Shift all four bytes of dividend 16 bits to the right and clear
*     both bytes of the temporary remainder location
*
        MOV     DIVIDEND+1,DIVIDEND+3   ;shift dividend lsb
        MOV     DIVIDEND,DIVIDEND+2     ;shift 2nd byte of dividend
        MOV     DIVIDEND-1,DIVIDEND+1   ;shift 3rd byte of dividend
        MOV     DIVIDEND-2,DIVIDEND     ;shift dividend msb
        CLR     REMAINDER               ;zero remainder msb
        CLR     REMAINDER+1             ;zero remainder lsb
*
*     Shift each byte of dividend and remainder one bit to the left
*
SHFTLP  LDA     REMAINDER               ;get remainder msb
        ROLA                            ;shift remainder msb into carry
        ROL     DIVIDEND+3              ;shift dividend lsb
        ROL     DIVIDEND+2              ;shift 2nd byte of dividend
        ROL     DIVIDEND+1              ;shift 3rd byte of dividend
        ROL     DIVIDEND                ;shift dividend msb
        ROL     REMAINDER+1             ;shift remainder lsb
        ROL     REMAINDER               ;shift remainder msb
*
*     Subtract both bytes of the divisor from the remainder
*
        LDA     REMAINDER+1             ;get remainder lsb
        SUB     2,SP                    ;subtract divisor lsb from remainder lsb
        STA     REMAINDER+1             ;store new remainder lsb
        LDA     REMAINDER               ;get remainder msb
        SBC     1,SP                    ;subtract divisor msb from remainder msb
        STA     REMAINDER               ;store new remainder msb
        LDA     DIVIDEND+3              ;get low byte of dividend/quotient
        SBC     #0                      ;dividend low bit holds subtract carry
        STA     DIVIDEND+3              ;store low byte of dividend/quotient
*
*     Check dividend/quotient lsb. If clear, set lsb of quotient to indicate
*     successful subraction, else add both bytes of divisor back to remainder
*
        BRCLR   0,DIVIDEND+3,SETLSB     ;check for a carry from subtraction
                                        ;and add divisor to remainder if set
        LDA     REMAINDER+1             ;get remainder lsb
        ADD     2,SP                    ;add divisor lsb to remainder lsb
        STA     REMAINDER+1             ;store remainder lsb
        LDA     REMAINDER               ;get remainder msb
        ADC     1,SP                    ;add divisor msb to remainder msb
        STA     REMAINDER               ;store remainder msb
        LDA     DIVIDEND+3              ;get low byte of dividend
        ADC     #0                      ;add carry to low bit of dividend
        STA     DIVIDEND+3              ;store low byte of dividend
        BRA     DECRMT                  ;do next shift and subtract

SETLSB  BSET    0,DIVIDEND+3            ;set lsb of quotient to indicate
                                        ;successive subtraction
DECRMT  DBNZ    3,SP,SHFTLP             ;decrement loop counter and do next
                                        ;shift
*
*     Move 32-bit dividend into INTACC1.....INTACC1+3 and put 16-bit
*     remainder in INTACC2:INTACC2+1
*
        LDA     REMAINDER               ;get remainder msb
        STA     1,SP                    ;temporarily store remainder msb
        LDA     REMAINDER+1             ;get remainder lsb
        STA     2,SP                    ;temporarily store remainder lsb
        MOV     DIVIDEND,QUOTIENT       ;
        MOV     DIVIDEND+1,QUOTIENT+1   ;shift all four bytes of quotient
        MOV     DIVIDEND+2,QUOTIENT+2   ; 16 bits to the left
        MOV     DIVIDEND+3,QUOTIENT+3   ;
        LDA     1,SP                    ;get final remainder msb
        STA     INTACC2                 ;store final remainder msb
        LDA     2,SP                    ;get final remainder lsb
        STA     INTACC2+1               ;store final remainder lsb
*
*     Deallocate local storage, restore register values, and return from
*     subroutine
*
        AIS     #3                      ;deallocate temporary storage
        PULX                            ;restore x-reg value
        PULA                            ;restore accumulator value
        PULH                            ;restore h-reg value
        RTS                             ;return

; PCC compatible FLASH Programming Routine - I think
; When called it copies ALL ram_flash variable and tables into flash
        include "burner.inc"

;-------------------------------------------------------------------------------
; Boot-loader vector.table
        include "vector.inc"

; Flash data, including re-writable data modified by configurator.
        include "flash.inc"

; Lookup Tables
        org     $F100
        include "barofactor.inc"
        include "kpafactor.inc"
        include "thermfactor.inc"
        include "airdenfactor.inc"
        include "throttlefactor.inc"

; Only needed when burning new chips.
       include "boot_r12.asm"

;-------------------------------------------------------------------------------

        end

