/* EFI main program */ /*** Program Overview ***/ /* This program is intended to work with the following: - A block of user inputs downloaded one-time into battery-backed sram to the structure 'inputs'. - A serial communications port which, depending on a dip switch setting, can be connected to either a pc or a box which contains buttons and an lcd display. The buttons are for instant test tuning of the fuel, spark and idle speed control. Each button push richens/ leans/ adv/ retard x%. This is lost on shutdown, but can be entered into sram later thru a pc. The lcd is for displaying problems such as temp overheating, etc. - An MC 68332ACFC20 cpu board - A sensor board with the following components: 2 MC145051P A/Ds, 1 MC 33298 Octal Serial chip (for relays), 1 UDN2993B stepper motor chip, 2 LM1815s (for crank/ cam position sensors) - An injector driver board - An MSD type ignition system triggered by the 68332 acting as a breaker point. The program is structured to perform engine calculations for each cylinder firing. A simple program flow is as follows: - Initialize - Get synch (when added crank tooth is sensed) ---> - calculate rpm | - read sensors | - calculate warmup, acceleration enrichments | - set idle speed | -->- for each cylinder: | | - check for exceeding rev limits | | - calculate, set fuel inj timing, pw | | - calculate spk advance (set in interrupt) | | | ----------------| | - check for out of synch/ re-synch | -----------------| Communications with the serial interface are handled as interrupts. */ struct simregs { /* sim registers */ volatile unsigned short int sreg[59]; }; #define sr (*(struct simregs *)0xFFFA00) /* address of 1st sim register */ struct tpuregs { /* tpu registers */ volatile unsigned short int treg[17]; }; #define tr (*(struct tpuregs *)0xFFFE00) /* address of 1st tpu register */ #define HSR_REG (volatile unsigned long * const) (0xFFFE18) struct tpuprams { /* tpu parameters */ volatile unsigned short int tpram[16][8]; }; #define tp (*(struct tpuprams *)0xFFFF00) /* address of 1st tpu parameter */ #define HL_PRD (volatile unsigned long * const) (0xFFFF18) #define TCR2_ERR (volatile unsigned char * const) (0xFFFFFC) #define TCR2_TOOTH (volatile unsigned char * const) (0xFFFFFD) #define SPK_RAM (volatile unsigned long * const) (0xFFFFA8) struct qsmregs { /* qsm registers */ volatile unsigned short int qreg[16]; }; #define qr (*(struct qsmregs *)0xFFFC00) /* address of 1st qsm register */ struct qsmram { /* qsm ram */ volatile unsigned short int qrec[16],qxmt[16]; /* A/D serial port data */ volatile unsigned char qcmd[16]; }; #define qrm (*(struct qsmram *)0xFFFD00) /* address of 1st qsm parameter */ struct inputs { /* User inputs */ char isc_enable; char no_cool_cof,no_mat_cof,no_map_cof,no_tps_cof,no_bat_cof,no_baro_cof; float cool_cof[5],mat_cof[5],map_cof[5],tps_cof[5],bat_cof[5],baro_cof[5], accg_cof[5]; short int cool_lim1,cool_lim11,cool_lim2,cool_def; short int mat_lim1,mat_lim2,mat_def; short int map_lim1,map_lim2,map_def; short int tps_lim1,tps_lim2,tps_def; short int bat_lim1,bat_lim2,bat_def; short int baro_lim1,baro_lim2,baro_def; short int inj_lim1,inj_lim2,t_inj_bkpt,t_inj_ramp,t_inj_close; short int bat_comp1_slpe,bat_comp2_slpe; unsigned char num_teeth,max_add_teeth,deg_per_tooth; short int no_temp,warm_tempa[30],warm_enrich_table[30],warm_spark_table[30], ism_pos_table[30],acc_temp_table[30]; unsigned long t_strt_enrich,t_strt_enrich_tail,t_aft_strt_enrich, t_aft_strt_enrich_min,t_aft_strt_enrich_tail,tmax_we; short int strt_enrich_const,strt_enrich_tempconst, aft_strt_enrich_const,aft_strt_enrich_tempconst; long t_xisc_const; short int ptemp_xisc_const; unsigned long t_burst,t_burst_tail,t_acc_tail; short int tps_acc,map_acc,acc0,acc_pw0,acc_burst; float acc_wght1,acc_wght2; unsigned short int ism_maxsteps,time_bet_steps1,time_bet_stepsn; unsigned char step_rate_cnt; short int rev_lim,del_rpm; float mass_air_coeff,veaf_table[20][20],spark_table[16][16],inj_flow_ramp, inj_flow_max,inj_flow_close,spk_adv_max; short int no_vmaps,no_vrpms,veaf_maps[20],veaf_rpms[20], no_smaps,no_srpms,spark_maps[16],spark_rpms[16]; short int pw_min,max_duty_cycle,cyl_fuel[8],cyl_spark[8]; short int stop_inj,exh_close,deg_reach_valve_air,inj_adv; float fuel_spd_cof; unsigned long t_fpump_shut; short int rpm_clr_choke,tps_clr_choke,start_inj_batch; }; #define ip (*(struct inputs *)0x0B0800) /* address of 1st user input variable - non_volatile ram */ struct constants { /* Constants */ short int sci_baud_pc; char lcd_c1[8],lcd_c2[8],lcd_c3[8],lcd_err[7]; }; #define cp (*(struct constants *)((long)&ip + sizeof(ip))) /* constants in non-volatile ram. */ struct tables { /* Lookup tables */ short int cool_table[1024],mat_table[1024]; }; #define tb (*(struct tables *)((long)&cp + sizeof(cp))) /* lookup tables in non-volatile ram */ #define NO_VPD_BYTES 74 /* no. bytes of vp display vars sent to pc; includes pad bytes required for cpu32 structure alignment */ struct variables { /* Program variables */ /* Start std display variables */ short int rpm; short int cool_temp_pc,mat_temp_pc,map_press_pc,map_dot_pc,tps_dot_pc, tps_pos_pc,bat_v100_pc,baro_press_pc,bat_comp1,bat_comp2; char check_eng[8]; short int pw_pc; float spark_advance_pc; short int cyl_no,pw_deg,start_inj_rel_pc; short int acc_enrich_pc,acc_pw; short int pm_fuel,pm_spark,pm_isc,pm_spare; char after_warmup; short int strt_enrich_pc,aft_strt_enrich_pc,warmup_enrich, warmup_spark,ism_pos_cmd_pc; unsigned long clock_time; short int pcpu_remain,accg; /* End std display variables */ short int cool_temp,mat_temp,map_press,map_dot,tps_dot,tps_pos, bat_v100,baro_press; short int pw; float spark_advance; short int start_inj_rel; short int acc_enrich; short int strt_enrich,aft_strt_enrich,ism_pos_cmd; short int *pm_ptr; char synch_flag,first_time,first_synch,lcd_eng; char no_fire,ism_init_flg,check_eng_old[8]; unsigned short int old_clock,elapsed_time; unsigned long curr_clock,old_clock2,lcd_time,clk_roltim; unsigned long cool_ftime,mat_ftime,map_ftime,tps_ftime, bat_ftime,baro_ftime,tps_map_time_old,tps_map_dt; unsigned short int value; unsigned short int tcr1; float scal_val; short int strt_enrich_temp,aft_strt_enrich_temp; short int acc_stat,isc_stat; short int cool_llim,map_old,tps_old; short int jx,ix,ixt,interp0,interpt,interp; short int acc_temp,acc_rate,acc_burst,ptemp_xisc; short int last_step,ism_pos0,ism_pos; unsigned long tase,t_acc,t_norm,t_acc_end,t_xisc; float prob,rand_no,air_den,interpf1,interpf2,veaf_corr; float d2p31m,d2p31,dseed; short int ixm,ixr,gamma_enrich; unsigned short int t_bkpt,t_ramp; float mass_air_fa,reqd_fuel,fuel_open_close; short int strt_inj_min,start_inj,crank_pos,angle_spk; float start_spk; char no_cyl_ahead,fpump_on,itc_interrupt; unsigned char tcr2_last,tooth_last,angle1,ratio1; unsigned short int tpram11_hi,tpram11; unsigned long t_fpump; long tmpint,store_spk[8],pw_max; char clr_choke; char ix_xmt,no_xmt,xmt_buf[80],pc_connect,isc_int_over; long **vbr_ptr; char *ip_ptr,*vp_ptr,*xptr; unsigned short int no_bytes,byte_offset,ibyte,sc_status; char xmt_status,rcv_status,rcv_ch,rcv_buf,qspi_svce_flag, ip_buff[8],batch_mode,cam_tooth,first_fuel[8], spk_L_H,all_spk_off,spk_off_sw[8],spk_cyl,cam_last, first_crank,first_spk; short int sitmp,sci_ix,exc_ix,accg_old,spk_ix; unsigned long t_last_cpu; unsigned long *coherent_ptr; float ftmp,spk_adv_last; unsigned short int usitmp; short no_dist_vfies; /* floating point stuff */ long fp_expn,fp_int_out; unsigned long fp_mantissa; unsigned long *fp_ptrf; float fp_flt_out; short fp_ix,fp_sign; /* end floating point stuff */ }; #define vp (*(struct variables *)((long)&tb + sizeof(tb))) /* prog variables in non-vol ram */ #define CLK_FREQ 168 /* CPU clock freq in MHz x 10 */ #define TPS_MAP_DT 50000 /* Sample period(usec) for tps, mapdot */ #define DST_VFIES_THRESH 3 /* n full cycles (2n revs) */ void spark_interrupt(void); void spark_intrpt2(void); void crank_interrupt(void); void crank_intrpt2(void); void sm_interrupt(void); void sm_intrpt2(void); void sci_interrupt(void); void sci_intrpt2(void); void usr_clock(void); void clock_interrupt(void); void clock_intrpt2(void); void qspi_service(void); float randu(void); float int_2_flt(long i,short i_type,short i_sign); long flt_2_int(float f); void excptn_interrupt(void); efi() { /* System initialization */ vp.vbr_ptr = (long **)0x0B0000; /* Set = address of cpu vector base register */ /* Set addresses of interrupt handlers: 0 - 63 cpu interrupts 64 - 79 tpu (16 channels) 80 - 81 qsm (sci, qspi) */ for (vp.ix=2; vp.ix <= 8; vp.ix++) { /* system errors */ *(vp.vbr_ptr + vp.ix) = (long *)excptn_interrupt; } for (vp.ix=32; vp.ix <= 47; vp.ix++) { *(vp.vbr_ptr + vp.ix) = (long *)excptn_interrupt; } *(vp.vbr_ptr + 65) = (long *)crank_interrupt; *(vp.vbr_ptr + 74) = (long *)spark_interrupt; *(vp.vbr_ptr + 75) = (long *)sm_interrupt; *(vp.vbr_ptr + 77) = (long *)clock_interrupt; *(vp.vbr_ptr + 80) = (long *)sci_interrupt; /* Configure sim registers */ sr.sreg[0] = 0x00CF; /* supv=1, mm=1, iarb=F */ sr.sreg[2] = 0x7F00; /* set clock freq= 16.777 MHz */ /* Set up Port C pins for knock int/hold, stepper enable A/B */ sr.sreg[35] &= 0xFFF0; /* CS6=PC3 = knock int/hold; CS7=PC4 = stepper enable. Both o/p pins. */ sr.sreg[32] &= 0xFFF7; /* Set knock to hold */ if(ip.isc_enable) sr.sreg[32] |= 0x0010; /* Enable stepper motor */ /* Set up Port E pins for control dip switches */ sr.sreg[11] &= 0xFF0F; /* clear pins 4-7 to be i/o pins */ sr.sreg[10] &= 0xFF0F; /* clear pins 4-7 to be input pins */ /* Configure tpu channel registers */ tr.treg[0] = 0x40CC; /* Set clock to 1 microsec step, iarb=C */ tr.treg[9] = 0xCCBA; /* Set functions for channels 3 to 0 */ tr.treg[8] = 0xCCCC; /* Set functions for channels 7 to 4 */ tr.treg[7] = 0xDCCC; /* Set functions for channels 11 to 8 */ tr.treg[6] = 0x00ED; /* Set functions for channels 15 to 12 */ tr.treg[4] = 0x0640; /* Set tpu interrupt config: req lev= 6, base vector no.=64(=0x40) */ tr.treg[5] = 0x0000; /* Initial interrupt enable options for channels 15 to 0 */ tr.treg[11] = 0x5551; /* Set host sequence registers for channels 7 to 0. ITC is continuous, no links; PMA is added tooth, bank mode; PSPs are angle/ time mode. */ tr.treg[10] = 0x0015; /* Set host sequence registers for channels 15 to 8. */ /* Clear all injector and spark turn-on pins */ for (vp.ix=2; vp.ix <= 10; vp.ix++) { tp.tpram[vp.ix][0] = 0x1AB2; /* channel ctl - psc to force pins low */ } tr.treg[13] = 0xFFF0; /* Set hsr to force mode to turn off channels 7 to 2 */ tr.treg[12] = 0x003F; /* Set hsr to force mode to turn off channels 10 to 8 */ tr.treg[15] = 0xFFF0; /* Set priority levels for channels 7 to 2 */ tr.treg[14] = 0x097F; /* Set priority levels for channels 13 to 8 */ /* Configure tpu channel parameters */ /* Channel 0 - ITC/Distributor signal */ tp.tpram[0][0] = 0x0047; /* channel ctl - psc, pac, tbs */ tp.tpram[0][2] = 0x0001; /* max count=1: only detect 1 transition */ /* Channel 1 - PMA/Crank signal */ vp.tpram11_hi = ip.max_add_teeth << 8; /* Channel 11 - SM/Stepper motor - master */ tp.tpram[11][0] = 0x0082; /* channel ctl - psc, pac, tbs */ tp.tpram[11][1] = 0x3333; /* Pin control */ tp.tpram[11][4] = 0x0001; /* mod_cnt=0, nxt_step_rate=1 */ tp.tpram[11][5] = (ip.step_rate_cnt << 8) + 12; /* last_sec_chan=12 */ /* Channel 12 - SM/Stepper motor - slave */ tp.tpram[12][0] = 0x0081; /* channel ctl - psc, pac, tbs */ tp.tpram[12][1] = 0x9999; /* Pin control */ tp.tpram[12][2] = ((ip.time_bet_steps1 - ip.time_bet_stepsn) * (long)100)/ (ip.step_rate_cnt - 1); /* =step_cntl0 */ tp.tpram[12][3] = (ip.time_bet_steps1 * 100) + (2 * tp.tpram[12][2]); /* =step_cntl1 */ /* Set initial position of idle speed motor (ism) */ if(ip.isc_enable) { /* Start up SM channels */ tr.treg[12] = 0x0280; /* Set host service requests(inits) for channels 11, 12. Note: always write hsrs as single value - no mask. (See p2-14 of tpu manual) */ while(tr.treg[12] & 0x03C0); /* Wait til hsrs serviced */ /* Set ism to 0 position - all way open */ tp.tpram[11][2] = ip.ism_maxsteps; /* Set current pos - assume all way closed */ tp.tpram[11][3] = 0; /* Set desired position */ vp.ism_init_flg = 0; tr.treg[16] = (tr.treg[16] & 0xF7FF); /* Clear interrupt status register */ tr.treg[5] |= 0x0800; /* Set interrupt enable for channel 11 */ vp.isc_int_over = 0; /* = 1 indicates isc interrupt complete */ tr.treg[12] = 0x00C0; /* HSR step request: When open all way, interrupt routine will close to coldest idle position. Note: Setting register to 00C0 will NOT 0 out any bits on other channel hsrs (see p 2-12) */ } /* end if(isc enable) */ /* Configure qsm registers */ qr.qreg[0] = 0x4088; /* qsm configuration, iarb=8 */ qr.qreg[2] = 0x2A50; /* Set qsm interrupt config: req lev= 5 for A/D, =2 for comm, base vector no.=80(=0x50) for sci (lcd/pc), =81 for qspi (a/d) */ /* Program knock sensor chip */ /* End programming knock sensor chip */ qr.qreg[10] = 0x0078; /* qsm pin defaults: PCS3-0 = 1 */ qr.qreg[11] = 0x7F7E; /* qsm pin assignments, directions */ qr.qreg[12] = 0xA828; /* qspi ctl: master, 10 bits, sck=209kHz, cpol=0; set cpha for A/D initially */ qr.qreg[13] = 0x4710; /* qspi ctl: spe=0, dsclk=71(4.4 microsec delay before start of xfer), dtl=16 (32 microsec delay to finish a/d conversion assuming interleaving of 2 a/d) Timing: 4.4 setup + 10 x 5 microsec to xmit 10 bits @ 200 kHz sck = 54 microsec, 86 microsec conversion delay. 86 - 54 = 32 microsec with interleaving. */ qr.qreg[14] = 0x2000; /* qspi ctl: no cpu interrupt, wrto=1, rest =0 initially */ qr.qreg[15] = 0; /* qspi ctl (no halt) & status registers */ /* Set qsm command & xmit ram */ qrm.qcmd[0] = 0x0E; /* Ser Switch (PCS0) - f/pump sw */ /* Interleave A/Ds so don't have to wait for conversion delay on 1st A/D chip (dt=0) */ qrm.qcmd[1] = 0x5D; /* A/D #1, AN0 (PCS1) - MAP. Goes to qrec[3] */ qrm.qcmd[2] = 0x77; /* A/D #2, AN0 (PCS3) - TPS. Goes to qrec[4] */ qrm.qcmd[3] = 0x5D; /* A/D #1, AN1 (PCS1) - BARO. Goes to qrec[5] */ qrm.qcmd[4] = 0x77; /* A/D #2, AN1 (PCS3) - COOL. Goes to qrec[6] */ qrm.qcmd[5] = 0x5D; /* A/D #1, AN2 (PCS1) - MAT. Goes to qrec[7] */ qrm.qcmd[6] = 0x77; /* A/D #2, AN2 (PCS3) - KNOCK. Goes to qrec[8] */ qrm.qcmd[7] = 0x5D; /* A/D #1, AN3 (PCS1) - ACCELG. Goes to qrec[9] */ qrm.qcmd[8] = 0x77; /* A/D #2, AN3 (PCS3) - Spare. Goes to qrec[10] */ qrm.qcmd[9] = 0x5D; /* A/D #1, AN4 (PCS1) - BAT. Goes to qrec[11] */ qrm.qcmd[10] = 0x77; /* A/D #2, AN4 (PCS3) - O2. Goes to qrec[12] */ qrm.qcmd[11] = 0x5D; /* A/D #1, AN5 (PCS1) - Spare. Goes to qrec[1] */ qrm.qcmd[12] = 0x77; /* A/D #2, AN5 (PCS3) - Spare. Goes to qrec[2] */ qrm.qxmt[0] = 0x00FF; /* Ser Sw - f/pump (off init, invrtd pin logic) */ vp.fpump_on = 0; for(vp.ix = 1; vp.ix <= 12; vp.ix++) qrm.qxmt[vp.ix] = ((vp.ix - 1)/ 2) * 64; /* Addresses for A/D channels: 4 msb of 10 bit word */ for(vp.ix = 13; vp.ix <= 15; vp.ix++) { qrm.qcmd[vp.ix] = 0; /* Spares */ qrm.qxmt[vp.ix] = 0; /* Spares */ } /* Turn on f/pump. Once on, will latch and stay on til turned off */ vp.qspi_svce_flag = 1; qspi_service(); /* Turn on A/D channels & wait til all data valid */ vp.qspi_svce_flag = 2; qspi_service(); /* Determine if pc(=1) or lcd/ Stamp(=0) connected. Won't accept change after this */ vp.pc_connect = 1 - ((sr.sreg[9] & 0x0020) >> 5); /* Read bit 5 of Port E (dipsw 1) - inverse logic*/ if(vp.pc_connect == 0) { qr.qreg[4] = 523200 / 2400; /* Fixed 2400 baud for sci w lcd/ Stamp */ vp.ix_xmt = 0; vp.no_xmt = 0; qr.qreg[5] = 0x002C; /* enable transmitter(te=1) to lcd, rcvr(rie=1,re=1) from Stamp: This will cause preamble byte to be sent. Set tie=0,since not yet ready to xmit data, and would get continuous interrupts as long as TDR empty. A rcvr interrupt may be received from preamble byte, but will be ignored since all 1s. */ } else { vp.ip_ptr = (char *)&ip; vp.vp_ptr = (char *)&vp; qr.qreg[4] = 523200 / cp.sci_baud_pc; /* Variable baud rate for sci w pc */ vp.ibyte = 0; vp.no_bytes = 0; vp.xmt_status = 1; /* xmtter on flag */ vp.rcv_status = -1; /* rcvr off flag */ qr.qreg[5] = 0x008C; /* enable xmtter (tie=1,te=1), rcvr(rie=0,re=1) for pc: This will cause preamble byte to be sent, and trigger TDRE interrupt when done. Since no_bytes=0, this will only result in clearing tie. The rcvr interrupt(rie=1) will then be enabled. */ } /* Initialize program variables */ vp.pm_fuel = 0; vp.pm_spark = 0; vp.pm_isc = 0; vp.pm_spare = 0; vp.pm_ptr = &vp.pm_fuel; vp.batch_mode = 0; vp.after_warmup = 0; vp.strt_inj_min = ip.stop_inj - flt_2_int(int_2_flt(ip.max_duty_cycle,16,1) * (float)7.2) - (ip.deg_reach_valve_air + ip.inj_adv); /* earliest time to start injecting - in crank deg */ vp.no_cyl_ahead = 2 - (vp.strt_inj_min / 90); if(vp.strt_inj_min > 0)vp.no_cyl_ahead--; if(vp.no_cyl_ahead < 0)vp.no_cyl_ahead = 0; /* no. of cylinders ahead for which fuel/ spark calculations done */ vp.first_synch = 1; vp.lcd_eng = 0; vp.accg_old = -777; /* Initialize random no. generator */ vp.d2p31m = (float)2.147484e09; /* d2p31m=(2**31) - 1 */ vp.d2p31 = (float)2.147485e09; /* d2p31=(2**31) or an adjusted value */ vp.dseed = (float)1.073242e09; /* Check for Synch. Assume 1 additional crank tooth & a 1-tooth reluctor set prior to crank tooth of TDC of #1 compression stroke. */ SYNCH: /* Start synch clock: interrupt driven clock with 30 ms resolution. Outputs clock_time (ms x 10). This clock convenient during synch - time always in clock time, don't need fine resoln. */ vp.synch_flag = 0; usr_clock(); vp.t_fpump = vp.clock_time; for(vp.ix = 0; vp.ix < 8; vp.ix++) { vp.spk_off_sw[vp.ix] = 0; } if(vp.batch_mode) { tr.treg[11] = 0x5555; /* no bank mode */ tp.tpram[0][1] = 0x000E; /* dummy bank address */ } else tp.tpram[0][1] = 0x0014; /* no links, bank address= PMA bank signal */ /* Initialize for check engine LCD msgs */ for(vp.ix = 0; vp.ix < 8; vp.ix++) { vp.check_eng[vp.ix] = 0; vp.check_eng_old[vp.ix] = 0; } vp.check_eng[0] = 9; /* Turn on check engine => not yet synched, */ tp.tpram[1][3] = 83 * 256 + (2 - vp.batch_mode) * ip.num_teeth + 2; /* Ratio=0.65 (83 decimal); tcr2_max_val=2xnum_teeth+2. 2x is for bank(sequential) mode. Extra 2 teeth can occur on 1st rev, if starter starts near additional tooth. */ vp.itc_interrupt = 0; tr.treg[15] &= 0xFFF3; /* Shut (priority=0) chan 1 (PMA) */ tp.tpram[1][0] = 0x0007; /* channel ctl - psc, pac, tbs. Must reset, since holds ref_time after hsr init */ tp.tpram[1][1] = vp.tpram11_hi + 1; /* initialize num_teeth = 1 */ tr.treg[16] &= 0xFFFE; /* Clear distrib interrupt status reg */ tr.treg[13] = 0x0005; /* Set host service requests(inits) for channels 0 and 1. */ tr.treg[15] |= 0x000F; /* Reset priority levels for chan 1 (PMA) and chan 0 (ITC) */ vp.tcr2_last = 0; while(tr.treg[13] & 0x000F); /* Wait til serviced */ SYNCHLOOP: /* Send 'ENG' to lcd to indicate not yet synched */ if(!vp.pc_connect && (vp.lcd_eng == 0) && ((vp.clock_time > 10000) || (vp.first_synch == 0))) { /* Need to wait 1 sec before use lcd */ qr.qreg[5] = (qr.qreg[5] & 0xFF7F); /* Clear xmt interrupt enable(tie=0). This prevents any activity in sci_interrupt during next few lines. (Don't want no_xmt or ix_xmt being altered). */ vp.xmt_buf[0] = 1; /* lcd 'clear' char */ vp.lcd_eng = 1; vp.xmt_buf[1] = 'E'; vp.xmt_buf[2] = 'N'; vp.xmt_buf[3] = 'G'; vp.check_eng_old[0] = vp.check_eng[0]; vp.no_xmt = 4; vp.lcd_time = 0; vp.ix_xmt = vp.first_synch; /* Don't want to send clear very 1st time - lcd starts clear and 1st char is throwaway char */ qr.qreg[7] = 254; /* puts lcd in instr mode (to accept 'clear') */ qr.qreg[5] = (qr.qreg[5] | 0x0080); /* Set xmt interrupt enable(tie=1) */ } /* Check for distributor signal (ITC) interrupt. Distrib. signal must arrive before added tooth. Aim for distrib pulse to occur ~17 deg(+/- 13 deg) BTDC of crank (= 8.5 deg +/- 6.5deg distrib adv). */ if((tr.treg[16] & 0x0001) == 1) { vp.itc_interrupt = 4; vp.tooth_last = (tp.tpram[0][4] & 0x00FF); /* = final_trans_time */ tr.treg[16] &= 0xFFFE; /* Clear distrib interrupt status reg */ } if(vp.itc_interrupt > 0) { /* Count interrupt flag down to 0 when reluctor 4 crank teeth past distrib TDC */ if(*TCR2_TOOTH != vp.tooth_last) { vp.tooth_last = *TCR2_TOOTH; /* = tcr2_value */ vp.itc_interrupt--; } } /* Check for crank additional tooth (PMA) */ if (*TCR2_ERR == 0xC0) { /* Code C0 error: always get this error at startup */ if (*TCR2_TOOTH != 0xFF) { if(*TCR2_TOOTH > (2-vp.batch_mode) * ip.num_teeth + 2) { vp.batch_mode = 1; goto SYNCH; } vp.tpram11 = vp.tpram11_hi + (*TCR2_TOOTH); /* Set num_teeth = tcr2_val(lo) to fool tpu and avoid code 80 error due to != num_teeth; this would require another rev to get rid of. Do coherently. */ tp.tpram[1][1] = vp.tpram11; /* = add teeth + numteeth */ } } else if (*TCR2_ERR == 0x80) { /* Code 80 error: - shouldn't get this error */ vp.batch_mode = 1; goto SYNCH; } else { /* err = 0: at or past TDC. Wait for distributor signal just prior to crank TDC (0 deg) */ if(vp.batch_mode || vp.itc_interrupt > 0) { /* Distrib interrupt => @ TDC of #1 compression stroke. If not in batch mode, want to wait til this occurs so tooth numbers will be correct */ tp.tpram[1][3] = 83 * 256 + ((2 - vp.batch_mode) * ip.num_teeth) + 1; /* Ratio=0.65 (83 decimal); tcr2_max_val = 2 x num_teeth + 1. Extra 2 teeth no longer needed */ goto HAVSYNCH; } } /* No PMA added tooth - check for crank rotation */ if(*TCR2_TOOTH != vp.tcr2_last) { vp.t_fpump = vp.clock_time; /* crank has moved - reset f/pump timer */ vp.tcr2_last = *TCR2_TOOTH; if(vp.fpump_on == 0) { /* F/pump off - turn it on */ vp.qspi_svce_flag = 3; qspi_service(); } } else { /* crank has not moved - check if time to shut off f/pump */ if((vp.fpump_on == 1) && (vp.clock_time - vp.t_fpump > ip.t_fpump_shut)) { vp.after_warmup = 0; vp.qspi_svce_flag = 3; /* Shut off pump */ qspi_service(); } } goto SYNCHLOOP; HAVSYNCH: vp.spk_adv_last = (float)0.; /* Set cylinder no. so that 1st time thru will do calculations for following cylinder */ vp.cyl_no = 0 + vp.no_cyl_ahead; /* Note: cyl_no is position in firing order - not the actual cyl no. */ vp.cam_tooth = 1; /* next time enter crnk intrpt (on added tooth), cam signal should be off */ tp.tpram[1][1] = vp.tpram11_hi + ip.num_teeth; /* = add teeth; numteeth */ /* Initialize immediately after synch */ vp.first_synch = 0; vp.t_last_cpu = 0; if(vp.fpump_on == 0) { /* F/pump off - turn it on */ vp.qspi_svce_flag = 3; qspi_service(); } /* Clear check engine to indicate eng synch */ if(vp.batch_mode) vp.check_eng[0] = 8; else vp.check_eng[0] = 0; if(vp.pc_connect == 0) { /* Note: want to clear lcd even if havn't sent 'eng' */ qr.qreg[5] = (qr.qreg[5] & 0xFF7F); /* Clear xmt interrupt enable(tie=0) */ vp.xmt_buf[0] = 1; /* lcd 'clear' char */ vp.lcd_eng = 0; vp.xmt_buf[1] = ' '; vp.no_xmt = 2; vp.ix_xmt = 0; qr.qreg[7] = 254; /* puts lcd in instr mode (to accept 'clear') */ qr.qreg[5] = (qr.qreg[5] | 0x0080); /* Set xmt interrupt enable(tie=1) */ } vp.tps_dot = 0; vp.map_dot = 0; vp.tps_map_time_old = 0; vp.cool_ftime = 0; vp.mat_ftime = 0; vp.map_ftime = 0; vp.tps_ftime = 0; vp.bat_ftime = 0; vp.baro_ftime = 0; vp.strt_enrich = 0; vp.aft_strt_enrich = 0; vp.warmup_enrich = 0; vp.warmup_spark = 0; vp.acc_stat = 0; vp.acc_enrich = 0; vp.acc_pw = 0; if(ip.isc_enable) vp.isc_stat = 0; else vp.isc_stat = 3; /* Channels 2 to 9 - PSP/1st thru 8th injectors in firing order */ for (vp.ix=2; vp.ix <= 9; vp.ix++) { vp.first_fuel[vp.ix - 2] = 1; tp.tpram[vp.ix][4] = 199; /* set bogus angle1 to inhibit firing til calculate proper angle 1 */ tp.tpram[vp.ix][0] = 0x1AB3; /* period address= PMA period, channel ctl - set psc to not force */ } /* Channel 10 - PSP/spark signal to MSD type box */ vp.all_spk_off = 1; tp.tpram[10][4] = 199; /* set bogus angle1 to inhibit firing til calculate proper angle 1 */ tp.tpram[10][0] = 0x1AB3; /* period address= PMA period, channel ctl - set psc to not force */ /* Set crank and spark interrupts */ vp.first_crank = 1; vp.first_spk = 1; if(vp.batch_mode == 0) tr.treg[5] |= 0x0002; /* crank */ tr.treg[5] |= 0x0400; /* spark (chan 10) */ vp.first_time = 1; vp.synch_flag = 1; /* causes clock to be reset to 0 */ /* Begin main calculation loop - one pass for each cylinder. Use current sensor data, but calculate fuel/ spark for n cylinders ahead, where n = no_cyl_ahead. Piston is currently @ TDC of compression stroke for cylinder =cyl_no - n; cylinders numbered 0 - 7 */ STARTCALC: /* Read clocks (32 bit times since synch) */ usr_clock(); /* Returns following times at this instant: curr_clock= cpu clock cycles, 1 cycle = 1 microsec; clock_time= 0.1 ms increment clock; no overflow for > 100 hrs */ /* Calculate rpm = (rev/delt) x 10**6 x 60 */ vp.rpm = 60000000 / ( ((*HL_PRD * 160) / CLK_FREQ) * ip.num_teeth); /* Read sensor inputs/ Check for failures */ /* Read throttle postion sensor & convert to engineering units */ vp.value = qrm.qrec[4]; vp.scal_val = ip.tps_cof[ip.no_tps_cof - 1]; vp.ix = ip.no_tps_cof - 2; do { /* polynomial curvefit */ vp.scal_val = vp.scal_val * int_2_flt(vp.value,16,0) + ip.tps_cof[vp.ix]; vp.ix--; } while (vp.ix >= 0); vp.tps_pos = flt_2_int(vp.scal_val); /* position as % x 10 fully open */ /* Check if out of limits */ if((vp.tps_pos >= ip.tps_lim1) && (vp.tps_pos <= ip.tps_lim2)) { vp.tps_ftime = vp.clock_time; /* Reset timer for duration of out of limits */ vp.check_eng[4] = 0; /* Turn off LCD msg */ } else { /* Broken */ vp.tps_pos = ip.tps_def; /* Set to default value */ if(vp.clock_time - vp.tps_ftime > 5000) /* Broken > 0.5 sec */ vp.check_eng[4] = 1; /* Turn on LCD msg */ } /* Calculate time derivative of tps - %/sec */ if(vp.first_time) vp.tps_old = vp.tps_pos; if(vp.curr_clock >= vp.tps_map_time_old) vp.tps_map_dt = (vp.curr_clock - vp.tps_map_time_old); else /* handle rollover */ vp.tps_map_dt = (0xFFFFFFFF - vp.tps_map_time_old) + 1 + vp.curr_clock; if(vp.tps_map_dt > TPS_MAP_DT) { vp.tps_dot = (100000*(vp.tps_pos - vp.tps_old))/(long)vp.tps_map_dt; vp.tps_old = vp.tps_pos; } vp.tps_pos_pc = vp.tps_pos; vp.tps_dot_pc = vp.tps_dot; /* Read manifold air pressure & convert to engineering units */ vp.map_press = flt_2_int(ip.map_cof[1] * int_2_flt(qrm.qrec[3],16,0) + ip.map_cof[0]); /* KPa x 10 */ /* Check if out of limits */ if((vp.map_press >= ip.map_lim1) && (vp.map_press <= ip.map_lim2)) { vp.map_ftime = vp.clock_time; /* Reset timer for duration of out of limits */ vp.check_eng[3] = 0; /* Turn off LCD msg */ } else { /* Broken */ vp.map_press = ip.map_def; /* Set to default value */ if(vp.clock_time - vp.map_ftime > 5000) /* Broken > 0.5 sec */ vp.check_eng[3] = 1; /* Turn on LCD msg */ } /* Calculate time derivative of map - KPa/sec */ if(vp.first_time) vp.map_old = vp.map_press; if(vp.tps_map_dt > TPS_MAP_DT) { vp.map_dot = (100000*(vp.map_press - vp.map_old))/(long)vp.tps_map_dt; vp.map_old = vp.map_press; vp.tps_map_time_old = vp.curr_clock; /* reset timer */ } vp.map_press_pc = vp.map_press; vp.map_dot_pc = vp.map_dot; /* Read coolant temperature & convert to engineering units */ vp.cool_temp = tb.cool_table[qrm.qrec[6]]; /* deg F x 10 */ /* Check if out of limits */ if(vp.clock_time < ip.tmax_we) /* Max warmup enrichment time (ms x 10) */ vp.cool_llim = ip.cool_lim1; /* Sensor lower limit - before warmup */ else vp.cool_llim = ip.cool_lim11; /* Coldest sensor should read after warmup */ if((vp.cool_temp >= vp.cool_llim) && (vp.cool_temp <= ip.cool_lim2)) { vp.cool_ftime = vp.clock_time; /* Reset timer for duration of out of limits */ vp.check_eng[1] = 0; /* Turn off LCD msg */ } else { /* Broken/ out of limits */ vp.cool_temp = ip.cool_def; /* Set to default value */ if(vp.clock_time - vp.cool_ftime > 5000) /* Broken/ out of limits > 0.5 sec */ vp.check_eng[1] = 1; /* Turn on LCD msg */ } vp.cool_temp_pc = vp.cool_temp; /* Read manifold air temperature & convert to engineering units */ vp.mat_temp = tb.mat_table[qrm.qrec[7]]; /* deg F x 10 */ /* Check if out of limits */ if((vp.mat_temp >= ip.mat_lim1) && (vp.mat_temp <= ip.mat_lim2)) { vp.mat_ftime = vp.clock_time; /* Reset timer for duration of out of limits */ vp.check_eng[2] = 0; /* Turn off LCD msg */ } else { /* Broken/ out of limits */ vp.mat_temp = ip.mat_def; /* Set to default value */ if(vp.clock_time - vp.mat_ftime > 5000) /* Broken/ out of limits > 0.5 sec */ vp.check_eng[2] = 1; /* Turn on LCD msg */ } vp.mat_temp_pc = vp.mat_temp; /* Read battery voltage & convert to engineering units */ vp.bat_v100 = flt_2_int(ip.bat_cof[1] * int_2_flt(qrm.qrec[11],16,0) + ip.bat_cof[0]); /* volts x 100 */ /* Check if out of limits */ if((vp.bat_v100 >= ip.bat_lim1) && (vp.bat_v100 <= ip.bat_lim2)) { vp.bat_ftime = vp.clock_time; /* Reset timer for duration of out of limits */ vp.check_eng[5] = 0; /* Turn off LCD msg */ } else { /* Out of limits */ vp.bat_v100 = ip.bat_def; /* Set to default value */ if(vp.clock_time - vp.bat_ftime > 5000) /* Out of limits > 0.5 sec */ vp.check_eng[5] = 1; /* Turn on LCD msg */ } vp.bat_v100_pc = vp.bat_v100; /* Read barometric air pressure & convert to engineering units */ vp.baro_press = flt_2_int(ip.baro_cof[1] * int_2_flt(qrm.qrec[5],16,0) + ip.baro_cof[0]); /* KPa x 10 */ /* Check if out of limits */ if((vp.baro_press >= ip.baro_lim1) && (vp.baro_press <= ip.baro_lim2)) { vp.baro_ftime = vp.clock_time; /* Reset timer for duration of out of limits */ vp.check_eng[6] = 0; /* Turn off LCD msg */ } else { /* Broken */ vp.baro_press = ip.baro_def; /* Set to default value */ if(vp.clock_time - vp.baro_ftime > 5000) /* Broken > 0.5 sec */ vp.check_eng[6] = 1; /* Turn on LCD msg */ } vp.baro_press_pc = vp.baro_press; /* Read accelerometer g's */ vp.accg = flt_2_int(ip.accg_cof[1] * int_2_flt(qrm.qrec[9],16,0) + ip.accg_cof[0]); /* +/-g x 1000 */ /* Calculate injector pulsewidth battery voltage compensations: Compensations are for injector turn-on times (bkpt and rampup). bat_comp1,2_slpe in microsec/ volt with 0 compensation @ 12 volts */ vp.bat_comp1 = -((long)ip.bat_comp1_slpe*(vp.bat_v100 - 1200))/100; /* microsec */ vp.bat_comp2 = -((long)ip.bat_comp2_slpe*(vp.bat_v100 - 1200))/100; /* microsec */ /* Calculate air density from air temperature & barometric pressure */ vp.air_den = ((float).0391568*int_2_flt((vp.baro_press - 31),16,1))/ ((int_2_flt(vp.mat_temp,16,1)/(float)10.) + (float)459.7); /* lbs / cu. ft. */ /* 31 KPa x 10 is correction for vapor pressure, assuming 75% humidity @ 85 deg F */ /* refresh clock - must be called at least every 65 ms */ usr_clock(); /* Calculate cranking/ after start/ warmup enrichments */ if(vp.after_warmup == 0) { /* Startup temperature-based enrichments */ if(vp.first_time == 1) { /* 1st time thru warmup loop */ vp.interp0 = ((long)100*(vp.cool_temp - ip.warm_tempa[0]))/ (ip.warm_tempa[ip.no_temp - 1] - ip.warm_tempa[0]); if(vp.interp0 > 100)vp.interp0 = 100; /* prevents neg enrichment */ /* Initial squirt (am't depending on temp @ startup) for a fixed, user input time of t_strt_enrich (ms x 10) */ vp.strt_enrich_temp = ip.strt_enrich_tempconst - ((ip.strt_enrich_tempconst * (long)vp.interp0)/100); /* gamma x 100 */ /* After start enrichment (@ rate dependent on temp @ startup) for a temperature dependent time of tase (ms x 10) */ vp.aft_strt_enrich_temp = ip.aft_strt_enrich_tempconst - ((ip.aft_strt_enrich_tempconst * (long)vp.interp0)/100); /* gamma x 100 */ vp.tase = ip.t_aft_strt_enrich - (((ip.t_aft_strt_enrich - ip.t_aft_strt_enrich_min)* (long)vp.interp0)/100); /* ms x 10 */ /* Following are 2 idle speed control parameters for extending fast idle beyond the point where coolant temp is normal - may be needed on very cold days, because oil temperature lags coolant and oil will not have reached normal viscosity, thereby acting as drag on engine */ vp.ptemp_xisc = ip.ptemp_xisc_const + (((100 - ip.ptemp_xisc_const)* (long)vp.interp0)/100); /* whole percent */ vp.t_xisc = ip.t_xisc_const - ((ip.t_xisc_const * vp.interp0)/100); /* ms x 10 */ } /* Time-based cranking/ starting enrichment */ if(vp.clock_time <= ip.t_strt_enrich) vp.strt_enrich = ip.strt_enrich_const + vp.strt_enrich_temp; else if(vp.clock_time <= (ip.t_strt_enrich + ip.t_strt_enrich_tail)) { vp.strt_enrich = ip.strt_enrich_const + vp.strt_enrich_temp; /* Tail off initial squirt */ vp.interp = (100*(vp.clock_time - ip.t_strt_enrich))/ip.t_strt_enrich_tail; vp.strt_enrich = vp.strt_enrich - ((vp.strt_enrich * (long)vp.interp)/100); } else vp.strt_enrich = 0; vp.strt_enrich_pc = vp.strt_enrich; /* Time-based after-start enrichment */ if(vp.clock_time <= vp.tase) vp.aft_strt_enrich = ip.aft_strt_enrich_const + vp.aft_strt_enrich_temp; else if(vp.clock_time <= (vp.tase + ip.t_aft_strt_enrich_tail)) { vp.aft_strt_enrich = ip.aft_strt_enrich_const + vp.aft_strt_enrich_temp; /* Tail off after start enrichment */ vp.interp = (100*(vp.clock_time - vp.tase))/ip.t_aft_strt_enrich_tail; vp.aft_strt_enrich = vp.aft_strt_enrich_temp - ((vp.aft_strt_enrich_temp * (long)vp.interp) / 100); } else vp.aft_strt_enrich = 0; vp.aft_strt_enrich_pc = vp.aft_strt_enrich; /* Warmup enrichment & spark */ vp.ixt = ip.no_temp; for(vp.ix = 1; vp.ix < ip.no_temp; vp.ix++) { if(vp.cool_temp < ip.warm_tempa[vp.ix]) { vp.ixt = vp.ix; break; } } vp.interpt = ((long)100*(vp.cool_temp - ip.warm_tempa[vp.ixt - 1]))/ (ip.warm_tempa[vp.ixt] - ip.warm_tempa[vp.ixt - 1]); if(vp.interpt < 0) vp.interpt = 0; if(vp.interpt > 100) vp.interpt = 100; vp.warmup_enrich = ip.warm_enrich_table[vp.ixt - 1] + (((long)vp.interpt*(ip.warm_enrich_table[vp.ixt] - ip.warm_enrich_table[vp.ixt - 1]))/100); /* gamma x 100 */ vp.warmup_spark = ip.warm_spark_table[vp.ixt - 1] + (((long)vp.interpt*(ip.warm_spark_table[vp.ixt] - ip.warm_spark_table[vp.ixt - 1]))/100); /* deg adv x 10 */ } /* End if(after_warmup == 0) */ /* Calculate acceleration enrichments */ /* Check for acceleration */ if((vp.tps_dot > ip.tps_acc) || (vp.map_dot > ip.map_acc)) { if(vp.acc_stat != 1) { /* 1st time thru for current acceleration */ vp.t_acc = vp.clock_time; vp.acc_stat=1; if(vp.after_warmup == 0) { /* calc warmup accel enrichment as function of temperature */ vp.acc_temp = ip.acc_temp_table[vp.ixt - 1] + (((long)vp.interpt * (ip.acc_temp_table[vp.ixt] - ip.acc_temp_table[vp.ixt - 1])) / 100); /* gamma x 100 */ } else vp.acc_temp = 0; } /* Calc accel enrichment based on user specified blend of throttle & map rates */ vp.acc_rate = flt_2_int(ip.acc_wght1*int_2_flt(vp.tps_dot,16,1) + ip.acc_wght2*int_2_flt(vp.map_dot,16,1)); /* gamma x 100 */ /* Monitor end of burst accel enrichment */ if((vp.clock_time - vp.t_acc) < ip.t_burst) { vp.acc_pw = ip.acc_pw0; /* microsec */ vp.acc_burst = ip.acc_burst; /* gamma x 100 */ } else if((vp.clock_time - vp.t_acc) < (ip.t_burst + ip.t_burst_tail)) { vp.interp = (100*(vp.clock_time - (vp.t_acc + ip.t_burst)))/ip.t_burst_tail; vp.acc_pw = ip.acc_pw0 - ((ip.acc_pw0 *(long)vp.interp)/100); /* microsec */ vp.acc_burst = ip.acc_burst - ((ip.acc_burst *(long)vp.interp)/100); /* gamma x 100 */ } else { /* Completely finished burst */ vp.acc_pw = 0; vp.acc_burst = 0; } /* Calc total (non-pw) accel enrichment */ vp.acc_enrich = ip.acc0 + vp.acc_temp + vp.acc_rate + vp.acc_burst; /* gamma x 100 */ } else { /* Acceleration trigger < threshold */ if(vp.acc_stat == 1) { /* Set up to wind down enrichment */ vp.t_acc_end = vp.clock_time; vp.acc_stat=2; vp.acc_pw = 0; } if(vp.acc_stat == 2) { /* Wind down enrichment */ if(vp.clock_time < (vp.t_acc_end + ip.t_acc_tail)) { vp.acc_enrich = ip.acc0 + vp.acc_temp; vp.interp = (100*(vp.clock_time - vp.t_acc_end))/ip.t_acc_tail; vp.acc_enrich = vp.acc_enrich - ((vp.acc_enrich *(long)vp.interp)/100); } else { vp.acc_enrich = 0; vp.acc_stat=0; /* Finished */ } } } /* End if(tps_dot > || map_dot > ) */ vp.acc_enrich_pc = vp.acc_enrich; /* Calculate and set (fast) idle speed control setting */ if(ip.isc_enable && vp.isc_int_over) { if(vp.isc_stat < 3) { if(vp.isc_stat == 0) { /* Calculate temperature dependent fast idle position - in steps; Note: ixt,interpt come from warmup enrich loop */ vp.ism_pos = ip.ism_pos_table[vp.ixt - 1] + (((long)vp.interpt*(ip.ism_pos_table[vp.ixt] - ip.ism_pos_table[vp.ixt - 1]))/100); /* Monitor when to start extended fast idle */ if(vp.cool_temp >= ((long)vp.ptemp_xisc*ip.warm_tempa[ip.no_temp - 1])/100) { vp.isc_stat = 1; vp.ism_pos0 = vp.ism_pos; } } if(vp.isc_stat == 1) { /* Monitor when reach normal operating temperature */ if(vp.cool_temp >= ip.warm_tempa[ip.no_temp - 1]) { vp.isc_stat = 2; vp.t_norm = vp.clock_time; } } if(vp.isc_stat == 2) { if(vp.t_xisc > 0) { vp.interp = (100*(vp.clock_time - vp.t_norm))/vp.t_xisc; vp.ism_pos = vp.ism_pos0 + (((ip.ism_pos_table[ip.no_temp - 1] - vp.ism_pos0) * (long)vp.interp) / 100); } /* Monitor when finished extended fast idle */ if(vp.clock_time >= (vp.t_norm + vp.t_xisc)) { vp.ism_pos = ip.ism_pos_table[ip.no_temp - 1]; vp.isc_stat = 3; /* Finished with isc, except + / - */ } } if(vp.clock_time > ip.tmax_we) { /* This test in case coolant sensor broken */ vp.ism_pos = ip.ism_pos_table[ip.no_temp - 1]; vp.isc_stat = 3; /* Finished with isc, except + / - */ } } /* end if(isc_stat < 3) */ /* Send ism_pos command to tpu */ vp.ism_pos_cmd = vp.ism_pos - vp.pm_isc; /* Include + / - commands */ if(vp.ism_pos_cmd < 0) vp.ism_pos_cmd = 0; if(vp.ism_pos_cmd > ip.ism_maxsteps) vp.ism_pos_cmd = ip.ism_maxsteps; if((vp.ism_init_flg == 1) && (vp.ism_pos_cmd != vp.last_step)) { /* send only if new pos != old pos */ tp.tpram[11][3] = vp.ism_pos_cmd; /* desired position */ vp.last_step = vp.ism_pos_cmd; if(ip.isc_enable < 2) sr.sreg[32] |= 0x0010; /* Enable stepper motor */ tr.treg[5] |= 0x0800; /* Set interrupt enable for channel 11 */ vp.isc_int_over = 0; tr.treg[12] = 0x00C0; /* hsr step request */ } } /* End if(isc_enable && isc_int_over) */ vp.ism_pos_cmd_pc = vp.ism_pos_cmd; /* Test for end of warmup */ if(vp.after_warmup == 0) { if((((vp.clock_time > (vp.tase + ip.t_aft_strt_enrich_tail)) && (vp.cool_temp >= ip.warm_tempa[ip.no_temp-1])) || (vp.clock_time > ip.tmax_we)) && (vp.isc_stat >= 3)) { vp.after_warmup = 1; vp.strt_enrich = 0; vp.aft_strt_enrich = 0; vp.warmup_enrich = 0; vp.warmup_spark = 0; } } /* Generate Check Engine/ accg LCD msgs. (Max of 16 + 8 = 24 msg chars) */ if(!vp.pc_connect) { vp.jx = 0; for(vp.ix = 0; vp.ix < 8; vp.ix++) { if(vp.check_eng[vp.ix] != vp.check_eng_old[vp.ix]) { vp.jx = 1; vp.check_eng_old[vp.ix] = vp.check_eng[vp.ix]; } } if(vp.accg != vp.accg_old) { vp.jx = 1; vp.accg_old = vp.accg; } if((vp.jx > 0) && ((vp.clock_time - vp.lcd_time) > 2000)) { /* check_eng status or accg have changed and not too soon to update again; send new msg to lcd */ vp.lcd_time = vp.clock_time; qr.qreg[5] = (qr.qreg[5] & 0xFF7F); /* Clear xmt interrupt enable(tie=0) */ vp.xmt_buf[0] = 1; /* lcd 'clear' char */ vp.ix_xmt = 0; for(vp.ix = 0; vp.ix < 8; vp.ix++) { if(vp.check_eng[vp.ix] > 0) { vp.xmt_buf[vp.ix_xmt+1] = cp.lcd_c1[vp.ix]; vp.xmt_buf[vp.ix_xmt+2] = cp.lcd_c2[vp.ix]; vp.xmt_buf[vp.ix_xmt+3] = cp.lcd_c3[vp.ix]; vp.xmt_buf[vp.ix_xmt+4] = ' '; vp.ix_xmt += 4; } if(vp.ix_xmt >= 16) break; /* no more msg after 16 chars */ } vp.xmt_buf[vp.ix_xmt+1] = 254; /* move to 2nd line & print pw, spk, accg */ vp.xmt_buf[vp.ix_xmt+2] = 192; vp.ix_xmt += 2; vp.sitmp = vp.pw_pc; vp.xmt_buf[vp.ix_xmt+1] = (vp.sitmp / 1000) + 48; vp.xmt_buf[vp.ix_xmt+2] = '.'; vp.xmt_buf[vp.ix_xmt+3] = ((vp.sitmp / 100) - (vp.sitmp / 1000) * 10) + 48; vp.xmt_buf[vp.ix_xmt+4] = ','; vp.ix_xmt += 4; vp.sitmp = flt_2_int(vp.spark_advance_pc); vp.xmt_buf[vp.ix_xmt+1] = (vp.sitmp / 10) + 48; vp.xmt_buf[vp.ix_xmt+2] = (vp.sitmp - (vp.sitmp / 10) * 10) + 48; vp.xmt_buf[vp.ix_xmt+3] = ','; vp.ix_xmt += 3; if(vp.accg < 0) { vp.xmt_buf[vp.ix_xmt+1] = '-'; vp.sitmp = -vp.accg; } else { vp.xmt_buf[vp.ix_xmt+1] = ' '; vp.sitmp = vp.accg; } vp.xmt_buf[vp.ix_xmt+2] = (vp.sitmp / 1000) + 48; vp.xmt_buf[vp.ix_xmt+3] = '.'; vp.xmt_buf[vp.ix_xmt+4] = ((vp.sitmp / 100) - (vp.sitmp / 1000) * 10) + 48; vp.xmt_buf[vp.ix_xmt+5] = ((vp.sitmp / 10) - (vp.sitmp / 100) * 10) + 48; vp.xmt_buf[vp.ix_xmt+6] = (vp.sitmp - (vp.sitmp / 10) * 10) + 48; vp.ix_xmt += 6; vp.no_xmt = vp.ix_xmt + 1; vp.ix_xmt = 0; qr.qreg[7] = 254; /* puts lcd in instr mode for 'clear' */ qr.qreg[5] = (qr.qreg[5] | 0x0080); /* Set xmt interrupt enable(tie=1) */ } } /* Begin cylinder calculation loop */ CYL_LOOP: usr_clock(); /* refresh clock */ /* Implement rev limiting: When rpm > rev lim, randomly skip firing cylinders, with prob of skipping increasing as rpm climbs. If hit rev_lim + del_rpm, stop all fires. */ vp.no_fire = 0; if(vp.rpm > ip.rev_lim) { vp.prob = (float)1.0 - (int_2_flt((vp.rpm - ip.rev_lim),16,1)/ int_2_flt(ip.del_rpm,16,1)); /* Generate uniformly distributed random no. bet 0 and 1 */ vp.rand_no = randu(); if(vp.rand_no > vp.prob) vp.no_fire = 1; } if(ip.cyl_fuel[vp.cyl_no] == 0) /* check for fuel cutoff to this cyl */ vp.no_fire = 1; /* Check for fuel cutoff to all cylinders when flooded or for compressn check */ vp.clr_choke = 0; if((vp.rpm < ip.rpm_clr_choke) && (vp.tps_pos > ip.tps_clr_choke)) vp.clr_choke = 1; /* clear choke when cranking with wot */ /* Calculate fuel pulsewidth */ /* Look up veaf correction factor = vol efficiency(fraction)/ ((A/F)/14.7) @ norm temp, non-accel */ for(vp.ixm = ip.no_vmaps - 1; vp.ixm > -1; vp.ixm--) { /* Start w highest index because will have least time for calculations at hi maps */ if(vp.map_press > ip.veaf_maps[vp.ixm]) { break; } } for(vp.ixr = ip.no_vrpms - 1; vp.ixr > -1; vp.ixr--) { /* Start w highest index because will have least time for calculations at hi rpms */ if(vp.rpm > ip.veaf_rpms[vp.ixr]) { break; } } vp.interpf1 = int_2_flt((vp.map_press - ip.veaf_maps[vp.ixm]),16,1) / int_2_flt((ip.veaf_maps[vp.ixm + 1] - ip.veaf_maps[vp.ixm]),16,1); vp.interpf2 = int_2_flt((vp.rpm - ip.veaf_rpms[vp.ixr]),16,1) / int_2_flt((ip.veaf_rpms[vp.ixr + 1] - ip.veaf_rpms[vp.ixr]),16,1); vp.veaf_corr = ((float)1. - vp.interpf1)*((float)1. - vp.interpf2)*ip.veaf_table[vp.ixm][vp.ixr] + vp.interpf1*((float)1. - vp.interpf2)*ip.veaf_table[vp.ixm+1][vp.ixr] + vp.interpf2*((float)1. - vp.interpf1)*ip.veaf_table[vp.ixm][vp.ixr+1] + vp.interpf1 * vp.interpf2 * ip.veaf_table[vp.ixm+1][vp.ixr+1]; /* Calculate lbs of air*F/A entering 1 cyl during intake stroke on this engine cycle */ vp.mass_air_fa = vp.air_den * ip.mass_air_coeff * int_2_flt(vp.map_press,16,1) * vp.veaf_corr * (int_2_flt(ip.cyl_fuel[vp.cyl_no],16,1) / (float)100.0); /* = # air*(F/A). Last term provides individual cyl tuning */ /* Sum up total enrichments. (14.7/(gamma_enrich/100) = enrichment air/fuel ratio) */ vp.gamma_enrich = 100 + vp.strt_enrich + vp.aft_strt_enrich + vp.warmup_enrich + vp.acc_enrich; /* gamma x 100 */ /* Calculate total lbs fuel required for this air. Include +/- button fuel enrichment */ vp.reqd_fuel = vp.mass_air_fa * (int_2_flt(vp.gamma_enrich,16,1) / (float)100.0) * ((float)1.0 + (int_2_flt(vp.pm_fuel,16,1)/(float)100.0)); /* Calculate fuel pulsewidth in microsec (= clock cycle). It is assumed ave. flow is: 0 up to breakpoint time, 0.33 x full flow during rampup time, 0.67 x full flow during closing. */ vp.t_bkpt = ip.t_inj_bkpt + vp.bat_comp1; vp.t_ramp = ip.t_inj_ramp + vp.bat_comp2; vp.fuel_open_close = ip.inj_flow_ramp * int_2_flt(vp.t_ramp,16,0) + ip.inj_flow_close * int_2_flt(ip.t_inj_close,16,1); if(vp.fuel_open_close <= vp.reqd_fuel) vp.pw = flt_2_int(int_2_flt((vp.t_bkpt + vp.t_ramp),16,0) + ((vp.reqd_fuel - vp.fuel_open_close) / ip.inj_flow_max)) + vp.acc_pw; else vp.pw = flt_2_int(int_2_flt(vp.t_bkpt,16,0) + ((vp.reqd_fuel / vp.fuel_open_close) * int_2_flt(vp.t_ramp,16,0)) ) + vp.acc_pw; /* Set limits on pulsewidth */ if(vp.pw < ip.pw_min) vp.pw = ip.pw_min; vp.pw_max = (ip.max_duty_cycle * 1200000) / vp.rpm; if(vp.pw_max > 32000) vp.pw_max = 32000; /* ~ short int limit */ if(vp.pw > vp.pw_max) vp.pw = vp.pw_max; /* Calculate start of fuel injection rel to 0 - 720 crank deg */ /* Calculate pulsewidth in crank deg */ vp.pw_deg = (6 * (long)vp.rpm * vp.pw) / 1000000; if(vp.pw_deg < (ip.stop_inj - ip.exh_close)) /* Can fit entire pulse bet exh close & stop_inj point. Latter is point, in 0 - 720 crank deg with 0 = start of power stroke, after which don't want fuel being injected, because it will hit closed or almost closed intake valve. Want to inject as early as possible after intake opens, for best mixing, but avoid injecting while exhaust open, if possible. */ vp.start_inj_rel = ip.exh_close; /* Start injecting @ exh_close */ else /* Can't fit pulse */ vp.start_inj_rel = ip.stop_inj - vp.pw_deg; /* Back off from stop inj point */ /* Correct for delay for air/ fuel to travel from injector port to valve. Also add user tuning injection advance. */ vp.sitmp = flt_2_int(ip.fuel_spd_cof * int_2_flt(vp.rpm,16,1) ); if(vp.sitmp > ip.deg_reach_valve_air) vp.sitmp = ip.deg_reach_valve_air; /* sitmp is min of the 2 */ vp.start_inj_rel -= (vp.sitmp + ip.inj_adv); /* Takes into account fastest of air speed/ fuel speed thru port. => min delay, min advance. If more needed, use inj_adv. start_inj_rel in crank deg rel to TDC of power stroke of cyl_no */ if(vp.batch_mode) { vp.start_inj_rel = ip.start_inj_batch; vp.pw /= 2; } vp.start_inj_rel_pc = vp.start_inj_rel; /* Update for no. of cylinder we are calculating & cnvt to abs crank deg */ vp.start_inj = vp.cyl_no * (short int)90 + vp.start_inj_rel; if(vp.start_inj >= 720) vp.start_inj -= 720; if(vp.batch_mode) { if(vp.start_inj >= 360) vp.start_inj -= 360; } /* Set injector fire command */ vp.angle1 = (10 * vp.start_inj) / ip.deg_per_tooth; /* compute tooth (0-95) just before fire */ vp.ratio1 = 128 + ((128 * ((10 * vp.start_inj) - ((short int)vp.angle1 * ip.deg_per_tooth))) / ip.deg_per_tooth); /* compute additional fraction of tooth til fire. Will be no. from 1.0 to 1.99 (*128). (See p 3-74 of tpu manual for rationale.) */ if(vp.angle1 > ip.num_teeth) vp.angle1++; /* compensate for tcr2 incremented after pass xtra tooth */ /* correct (decrement) for ratio1 > 1 (> 128) */ if(vp.angle1 > 0) vp.angle1--; else vp.angle1 = (2 - vp.batch_mode) * ip.num_teeth - vp.batch_mode; if((vp.no_fire > 0) || (vp.clr_choke > 0)) vp.pw = 0; /* Inhibit fuel */ vp.pw_pc = vp.pw; /* set chan [2+cyl_no] to auto fire when reach angle1 tooth + ratio1 */ vp.coherent_ptr = (unsigned long *)&tp.tpram[2+vp.cyl_no][4]; vp.tmpint = ((unsigned long)(vp.ratio1 << 24) + (vp.angle1 << 16)) + vp.pw; /* xfer to registers coherently */ *vp.coherent_ptr = vp.tmpint; /* Set hsr inits for PSP. Need to do because when set angle1 = 199, it is set in match reg, & never gets cleared unless match occurs */ if(vp.first_fuel[vp.cyl_no] == 1) { vp.first_fuel[vp.cyl_no] = 0; *HSR_REG = (0x00000020 << 2*vp.cyl_no); /* This sets hsr in appropriate PSP channel in treg[13] or [12] */ } /* Calculate spark advance */ /* Look up advance in table */ for(vp.ixm = ip.no_smaps - 1; vp.ixm > -1; vp.ixm--) { /* Start w highest index because will have least time for calculations at hi maps */ if(vp.map_press > ip.spark_maps[vp.ixm]) { break; } } for(vp.ixr = ip.no_srpms - 1; vp.ixr > -1; vp.ixr--) { /* Start w highest index because will have least time for calculations at hi rpms */ if(vp.rpm > ip.spark_rpms[vp.ixr]) { break; } } vp.interpf1 = int_2_flt((vp.map_press - ip.spark_maps[vp.ixm]),16,1) / int_2_flt((ip.spark_maps[vp.ixm + 1] - ip.spark_maps[vp.ixm]),16,1); vp.interpf2 = int_2_flt((vp.rpm - ip.spark_rpms[vp.ixr]),16,1) / int_2_flt((ip.spark_rpms[vp.ixr + 1] - ip.spark_rpms[vp.ixr]),16,1); vp.spark_advance = ((float)1. - vp.interpf1)*((float)1. - vp.interpf2)*ip.spark_table[vp.ixm][vp.ixr] + vp.interpf1*((float)1. - vp.interpf2)*ip.spark_table[vp.ixm+1][vp.ixr] + vp.interpf2*((float)1. - vp.interpf1)*ip.spark_table[vp.ixm][vp.ixr+1] + vp.interpf1 * vp.interpf2 * ip.spark_table[vp.ixm+1][vp.ixr+1]; /* deg adv in crank - deg btdc */ /* Include individual cyl tuning, warmup, and +/- buttons */ vp.spark_advance = (vp.spark_advance * (int_2_flt(ip.cyl_spark[vp.cyl_no],16,1) / (float)100.0)) + (int_2_flt(vp.warmup_spark,16,1) / (float)10.) + int_2_flt(vp.pm_spark,16,1); /* Total deg adv in crank - deg btdc */ /* Set user limits on max advance */ if(vp.spark_advance > ip.spk_adv_max) vp.spark_advance = ip.spk_adv_max; /* Add extra advance to account for MSD firing on rising edge of null */ vp.spark_advance += (float).12 * int_2_flt(ip.deg_per_tooth,8,0); /* spk pulse slightly longer than 1 tooth */ /* Set program limits on advance */ if(vp.spark_advance < (float)0.1) vp.spark_advance = (float)0.1; /* Latest spark is (0.1 - 1.2xdeg/tooth) deg ATDC */ if(vp.spark_advance > (float)89.9) vp.spark_advance = (float)89.9; /* Earliest spark is (89.9 - 1.2xdeg/tooth) deg BTDC */ /* Ensure don't try to fire spk for this cyl til last has finished */ vp.ftmp = ((float)90.0 - ((float).12 * int_2_flt(ip.deg_per_tooth,8,0) - vp.spk_adv_last)) - (float)1.0; if(vp.spark_advance > vp.ftmp) vp.spark_advance = vp.ftmp; vp.spk_adv_last = vp.spark_advance; vp.spark_advance_pc = vp.spark_advance; /* Calculate start of spark relative to 0 - 720 crank deg */ vp.start_spk = (float)720. - vp.spark_advance; /* Update for cylinder we are calculating & cnvt to abs crank deg */ vp.start_spk += int_2_flt(vp.cyl_no * (short int)90,16,1); if(vp.start_spk >= (float)720.0) vp.start_spk -= (float)720.0; if(vp.batch_mode) { if(vp.start_spk >= (float)360.0) vp.start_spk -= (float)360.0; } /* Compute tooth (0-95) just before fire */ vp.angle_spk = flt_2_int(vp.start_spk) * 10; vp.angle1 = vp.angle_spk / ip.deg_per_tooth; /* Compute additional fraction of tooth til fire. Will be no. from 0.0 to 0.99 (*128) */ vp.ratio1 = (128 * (vp.angle_spk - ((short int)vp.angle1 * ip.deg_per_tooth))) / ip.deg_per_tooth; /* compensate for tcr2 incremented after pass xtra tooth */ if(vp.angle1 > ip.num_teeth) vp.angle1++; /* duration = 1.2 x dt between teeth - microsec using latest rpm. (This avoids problem on p3-74 of tpu manual) */ vp.usitmp = (72000000 / ip.num_teeth) / vp.rpm; /* Store spark fire command */ vp.store_spk[vp.cyl_no] = ((unsigned long)(vp.ratio1 << 24) + (vp.angle1 << 16)) + vp.usitmp; if(vp.first_spk) { vp.first_spk = 0; /* Load spark array with valid values 1st time thru */ for(vp.ix = 0; vp.ix < 8; vp.ix++) { if(vp.ix != vp.cyl_no) { vp.sitmp = vp.angle_spk + (vp.ix - vp.cyl_no) * 900; if(vp.sitmp < 0) vp.sitmp += 7200; if(vp.sitmp >= 7200) vp.sitmp -= 7200; if(vp.batch_mode) { if(vp.sitmp >= 3600) vp.sitmp -= 3600; } vp.angle1 = vp.sitmp / ip.deg_per_tooth; if(vp.angle1 > ip.num_teeth) vp.angle1++; vp.store_spk[vp.ix] = ((unsigned long)(vp.ratio1 << 24) + (vp.angle1 << 16)) + vp.usitmp; } } } if(vp.no_fire > 0) vp.spk_off_sw[vp.cyl_no] = 1; /* inhibit fire */ else { vp.spk_off_sw[vp.cyl_no] = 0; /* restore fire */ if(vp.all_spk_off) { /* Re-initialize spark_interrupt */ vp.spk_cyl = vp.cyl_no - 1; if(vp.spk_cyl < 0) vp.spk_cyl = 7; vp.spk_L_H = 0; spark_intrpt2(); } } /* Check for out of synch condition */ if(*TCR2_ERR) { /* Re-initialize and re-synch */ tr.treg[5] &= 0xFFFD; /* kill crank interrupt */ tr.treg[5] &= 0xFBFF; /* kill spark interrupt */ tr.treg[15] = (tr.treg[15] & 0x000F); /* Zero priority for channels 7 to 2 */ tr.treg[14] = (tr.treg[14] & 0xFFC0); /* Zero priority for channels 10 to 8 */ for(vp.ix=2; vp.ix <= 10; vp.ix++) { tp.tpram[vp.ix][0] = 0x1AB2; /* channel ctl - psc to force pins low */ } tr.treg[13] = 0xFFF0; /* Set hsr to force mode to turn off channels 7 to 2. */ tr.treg[12] = 0x003F; /* Set hsr to force mode to turn off channels 10 to 8. */ tr.treg[15] = (tr.treg[15] | 0xFFF0); /* Reset priority for channels 7 to 2 */ tr.treg[14] = (tr.treg[14] | 0x003F); /* Reset priority for channels 10 to 8 */ goto SYNCH; } /* Finished calculations for this cylinder. Update cylinder (channel) no. for which doing the calculations */ vp.cyl_no++; if(vp.cyl_no < 8) goto CYL_LOOP; vp.cyl_no = 0; vp.first_time = 0; /* Calculate %cpu remaining per cyl firing */ vp.pcpu_remain = 100 - (((long)(vp.clock_time - vp.t_last_cpu) * vp.rpm) / 12000); vp.t_last_cpu = vp.clock_time; goto STARTCALC; } void crank_interrupt(void); asm (" .text .align 2 .globl crank_interrupt crank_interrupt: MOVEML %D0-%D7/%A0-%A6,%SP@- jbsr crank_intrpt2 MOVEML %SP@+,%D0-%D7/%A0-%A6 RTE"); /* Restore registers */ void crank_intrpt2(void) { /* check for err condition - out of synch */ if(*TCR2_ERR) { /* kill crank interrupt or else will keep coming here on every tooth */ tr.treg[5] &= 0xFFFD; return; /* ignore error condition - will handle in main */ } /* read & clear crank interrupt status register */ tr.treg[16] &= 0xFFFD; if(vp.first_crank) { vp.cam_last = 1 - vp.cam_tooth; vp.first_crank = 0; vp.no_dist_vfies = 0; } else { if(vp.no_dist_vfies >= DST_VFIES_THRESH) { /* start normal interrupt path */ vp.cam_tooth = 1 - vp.cam_tooth; if(!vp.cam_tooth) { /* not at true TDC */ tp.tpram[1][2] |= 0x0100; /* Set bank signal */ tp.tpram[1][1] = vp.tpram11_hi + (2 * ip.num_teeth) + 1; } else tp.tpram[1][1] = vp.tpram11_hi + ip.num_teeth; return; /* end normal interrupt path */ } vp.cam_last = vp.cam_tooth; vp.cam_tooth = (tr.treg[16] & 0x0001); } tr.treg[16] &= 0xFFFE; /* clear distrib interrupt status reg */ /* Interrupt occurred because have just hit added tooth. Check distrib interrupt alternating; if not, either distrib signal lost or noisy. */ if(vp.cam_tooth == vp.cam_last) { /* distrib signal fault */ vp.batch_mode = 1; /* Will stay in batch mode til re-start car */ vp.check_eng[0] = 8; tr.treg[5] &= 0xFFFD; /* kill future crank interrupts */ *TCR2_ERR = 0x80; /* Have to resynch to switch to batch mode, so force error so resynch as soon as possible */ return; } if(vp.cam_tooth) /* at true TDC */ vp.no_dist_vfies++; if(vp.no_dist_vfies >= DST_VFIES_THRESH) { /* Turn off ITC to make robust against dist failure */ tr.treg[15] &= 0xFFFC; /* zero ITC priority */ tp.tpram[0][1] = 0x000E; /* dummy ITC bank address */ } tp.tpram[1][1] = vp.tpram11_hi + (2 - vp.cam_tooth) * ip.num_teeth + (1 - vp.cam_tooth); /* = add teeth; numteeth */ return; } void spark_interrupt(void); asm (" .text .align 2 .globl spark_interrupt spark_interrupt: MOVEML %D0-%D7/%A0-%A6,%SP@- jbsr spark_intrpt2 MOVEML %SP@+,%D0-%D7/%A0-%A6 RTE"); /* Restore registers */ void spark_intrpt2(void) { /* read & clear interrupt status register */ tr.treg[16] &= 0xFBFF; /* Interrupts occur on L -> H and H -> L transitions; ignore the latter. */ vp.spk_L_H = 1 - vp.spk_L_H; if(vp.spk_L_H == 0) return; for(vp.spk_ix=0; vp.spk_ix < 8; vp.spk_ix++) { vp.spk_cyl++; if(vp.spk_cyl > 7) vp.spk_cyl = 0; if(!vp.spk_off_sw[vp.spk_cyl]) goto SET_ANGLE; } /* All sparks have been turned off - set bogus angle to inhibit further firings */ vp.all_spk_off = 1; tp.tpram[10][4] = 199; /* set bogus angle1 to inhibit firing */ return; SET_ANGLE: /* set ratio, angle and duration in register */ *SPK_RAM = vp.store_spk[vp.spk_cyl]; if(vp.all_spk_off) { /* Re-initialize spark_interrupt */ vp.all_spk_off = 0; vp.spk_L_H = 0; tr.treg[12] = 0x0020; /* Set hsr init for channnel 10 */ } return; } void sm_interrupt(void); asm (" .text .align 2 .globl sm_interrupt sm_interrupt: MOVEML %D0-%D7/%A0-%A6,%SP@- jbsr sm_intrpt2 MOVEML %SP@+,%D0-%D7/%A0-%A6 RTE"); /* Restore registers */ void sm_intrpt2(void) { /* This interrupt entered when SM init completed, or, when SM has reached ordered position */ tr.treg[5] &= 0xF7FF; /* disable interrupt */ tr.treg[16] &= 0xF7FF; /* read & clear intrpt stat reg */ if(!vp.ism_init_flg) { vp.ism_init_flg = 1; /* Set SM initialized flag */ /* Close ism to coldest idle position */ tp.tpram[11][3] = ip.ism_pos_table[0]; vp.last_step = tp.tpram[11][3]; tr.treg[5] |= 0x0800; /* Set interrupt enable for channel 11 */ vp.isc_int_over = 0; tr.treg[12] = 0x00C0; /* HSR step request */ } else { /* After SM init */ if(ip.isc_enable < 2) sr.sreg[32] &= 0xFFEF; /* Disable stepper motor */ vp.isc_int_over = 1; /* interrupt over, wait for next cmd */ } return; } void sci_interrupt(void); asm (" .text .align 2 .globl sci_interrupt sci_interrupt: MOVEML %D0-%D7/%A0-%A6,%SP@- jbsr sci_intrpt2 MOVEML %SP@+,%D0-%D7/%A0-%A6 RTE"); /* Restore registers */ void sci_intrpt2(void) { vp.sc_status = qr.qreg[6]; /* read sc status reg so tdre/ rdre can be cleared on next write/ read */ if(vp.pc_connect == 0 ) { /* Interface with lcd/ Stamp */ /* Note: don't have to disable xmit interrupt when recving or vice versa, because xmt & rcv on same (sci) interrupt. can't get 2nd interrupt til after RTE. (CPU manual, p6-1 to -16) */ if((vp.sc_status & 0x0040) != 0) { /* if(RDRF = 1) ,ie, rcvr full */ vp.sci_ix = (unsigned char)qr.qreg[7] - 1; /* read char from RDR. Need to do this, even if noise, to clear register */ if((vp.sc_status & 0x000F) == 0) { /* no noise on line, no data logjam */ /* Read + / - buttons: fuel buttons = 1% increase/ decrease in F/A ratio spark buttons = 1 crank deg advance/ retard in spark timing isc buttons = 1 step increase (open for faster idle)/ decrease (close for slower idle) */ if((vp.sci_ix >= 0) && (vp.sci_ix < 8)) { /* single button pushed */ if(vp.sci_ix - (2 * (vp.sci_ix / 2)) == 0) /* even => + button, increment */ (*(vp.pm_ptr + vp.sci_ix / 2))++; /* pointer to pm_fuel, pm_spark, pm_isc, pm_spare */ else /* odd */ (*(vp.pm_ptr + vp.sci_ix / 2))--; } else if((vp.sci_ix >= 8) && (vp.sci_ix < 12)) /* 2 buttons pushed together => zero counter */ *(vp.pm_ptr + (vp.sci_ix - 8)) = 0; } } /* End receive processing from Stamp */ if((vp.sc_status & 0x0100) != 0) { /* if(TDRE = 1) ,ie, xmttr empty */ if(vp.ix_xmt < vp.no_xmt) { /* have data to xmit */ qr.qreg[7] = vp.xmt_buf[vp.ix_xmt]; /* xmt next char(to TDR) */ vp.ix_xmt++; } else /* finished transmitting */ qr.qreg[5] &= 0xFF7F; /* clear xmt interrupt(tie=0) */ } /* End xmit processing to lcd */ } else { /* Interface with pc */ /* Receive commands, data from pc */ if(vp.rcv_status >= 0) { if((vp.sc_status & 0x000F) != 0) /* noise on line or data logjam */ goto NOISE; if(vp.rcv_status == 0) { vp.rcv_ch = qr.qreg[7]; /* receive 1st char (from RDR) */ vp.ibyte = 0; if(vp.rcv_ch == '$') { /* Download all user inputs from pc */ if(vp.synch_flag > 0) /* Can only do in synch loop */ goto NOISE; vp.byte_offset = 0; vp.no_bytes = sizeof(ip); vp.rcv_status = 3; } else if(vp.rcv_ch == '%') { /* Transmit standard display variables to pc */ vp.no_bytes = NO_VPD_BYTES; for(vp.sci_ix=0; vp.sci_ix < vp.no_bytes; vp.sci_ix++) /* load xmt buffer to ensure word & time tag coherence */ vp.xmt_buf[vp.sci_ix] = *(vp.vp_ptr + vp.sci_ix); vp.xptr = vp.xmt_buf; vp.xmt_status = 0; } else if(vp.rcv_ch == '/') { /* Transmit all user input variables to pc(to verify) */ vp.xptr = vp.ip_ptr; vp.no_bytes = sizeof(ip); vp.xmt_status = 0; } else if ((vp.rcv_ch == 'C') || (vp.rcv_ch == 'I') || (vp.rcv_ch == 'D') || (vp.rcv_ch == 'B')) { vp.rcv_status = 1; } else /* not valid character => corrupted by noise */ goto NOISE; } else if(vp.rcv_status == 1) { if(vp.rcv_ch == 'B') { /* button push */ /* Read + / - buttons */ vp.sci_ix = (unsigned char)qr.qreg[7] - 1; /* read char from RDR */ if((vp.sci_ix >= 0) && (vp.sci_ix < 8)) { /* single button pushed */ if(vp.sci_ix - (2 * (vp.sci_ix / 2)) == 0) /* even => + button, increment */ (*(vp.pm_ptr + vp.sci_ix / 2))++; /* pointer to pm_fuel, pm_spark, pm_isc, pm_spare */ else /* odd */ (*(vp.pm_ptr + vp.sci_ix / 2))--; } else if((vp.sci_ix >= 8) && (vp.sci_ix < 12)) /* 2 buttons pushed together => zero counter */ *(vp.pm_ptr + (vp.sci_ix - 8)) = 0; vp.rcv_status = 0; } else { /* not button push */ vp.rcv_status = 2; vp.rcv_buf = qr.qreg[7]; /* receive 2nd char (from RDR) */ vp.no_bytes = (vp.rcv_buf & 0xF0) >> 4; /* no bytes in most signif nibble of received char (will never be > 8) */ vp.byte_offset = (vp.rcv_buf & 0x0F) << 8; /* most signif nibble of offset in least signif nibble of received char */ } } else if(vp.rcv_status == 2) { vp.byte_offset += (unsigned char)qr.qreg[7]; /* receive 3rd char from RDR(rest of offset); max offset is 2**12 - 1 = 4095 bytes */ if(vp.rcv_ch == 'C') { /* Command to change a specific user input. Get new data */ if(((vp.byte_offset + vp.no_bytes) > sizeof(ip)) || (vp.no_bytes > 8)) goto NOISE; vp.rcv_status = 3; } if(vp.rcv_ch == 'I') { /* Request for a specific user input variable. Transmit the requested data */ if(((vp.byte_offset + vp.no_bytes) > sizeof(ip)) || (vp.no_bytes > 8)) goto NOISE; vp.xptr = vp.ip_ptr + vp.byte_offset; vp.xmt_status = 0; } if(vp.rcv_ch == 'D') { /* Request for a specific display variable. Transmit the requested data */ if(((vp.byte_offset + vp.no_bytes) > sizeof(vp)) || (vp.no_bytes > NO_VPD_BYTES)) goto NOISE; vp.xptr = vp.vp_ptr + vp.byte_offset; for(vp.sci_ix=0; vp.sci_ix < vp.no_bytes; vp.sci_ix++) /* load xmt buffer to ensure word & time tag coherence */ vp.xmt_buf[vp.sci_ix] = *(vp.xptr + vp.sci_ix); vp.xptr = vp.xmt_buf; vp.xmt_status = 0; } } else { /* rcv_status = 3: Change user input(s) byte by byte */ if(vp.no_bytes <= 8) vp.ip_buff[vp.ibyte] = qr.qreg[7]; /* Store in buffer to ensure whole word is transferred before returning from this interrupt */ else *(vp.ip_ptr + vp.byte_offset + vp.ibyte) = qr.qreg[7]; /* Transfer directly if entire ip structure being updated, since car won't be moving and since would take too long to xfer from buffer to ip before receiving verify request */ vp.ibyte++; if(vp.ibyte >= vp.no_bytes) { if(vp.no_bytes <= 8) { for(vp.ibyte=0; vp.ibyte < vp.no_bytes; vp.ibyte++) { /* Dump buffer */ *(vp.ip_ptr + vp.byte_offset + vp.ibyte) = vp.ip_buff[vp.ibyte]; } } vp.rcv_status = 0; /* wait to receive next command */ } } } /* End rcvr handling (rcv_status >= 0) */ /* Transmit data to pc for display */ if(vp.xmt_status >= 0) { if(vp.xmt_status == 0) { qr.qreg[5] = (qr.qreg[5] & 0xFFDF); /* Clear rcvr interrupt (rie=0).*/ vp.ibyte = 0; vp.xmt_status = 1; /* xmtter flag on */ vp.rcv_status = -1; /* rcvr flag off */ } if(vp.ibyte < vp.no_bytes) { qr.qreg[7] = *(vp.xptr + vp.ibyte); /* xmt a char to tdr */ vp.ibyte++; qr.qreg[5] |= 0x0080; /* turn on xmt interrupt(tie=1) */ } else { /* Finished xmt */ qr.qreg[5] &= 0xFF7F; /* clear xmt interrupt(tie=0) */ vp.xmt_status = -1; vp.rcv_status = 0; /* wait to receive next command */ qr.qreg[5] = (qr.qreg[5] | 0x0020); /* turn on rcvr interrupt (rie=1) */ } } /* End xmtter handling */ } return; NOISE: /* Disable any further rie, tie interrupts. Must restart to re-establish comm with pc. */ qr.qreg[5] = (qr.qreg[5] & 0xFF5F); return; } void usr_clock(void) { if(vp.synch_flag == 0) { /* Set up 30 ms, interrupt driven clock while trying to synch */ /* Channel 13 - OC Compare for clock */ tp.tpram[13][0] = 0x0093; /* channel ctl - psc, pac, tbs */ tp.tpram[13][1] = (30000 * CLK_FREQ) / 160; /* Offset = 30 ms */ tp.tpram[13][2] = 0x00D8; /* Ref address1 = Reference_ Time address. This appears to be only time that will be consistent. tcr1 & act_match time may not be exactly = Ref_time when match( >= ) occurs. */ vp.clock_time = 0; /* Set single shot (host seq=10) wo. interrupt to get tcr1, then put into Ref_time */ tr.treg[5] = (tr.treg[5] & 0xDFFF); /* disable interrupt */ tr.treg[10] = (tr.treg[10] | 0x0800); /* Set host sequence = 10 for read tcr1 only */ tr.treg[12] = 0x0400; /* Set hsr = 01 (host initiated pulse) */ while(tr.treg[12] & 0x0C00); /* Wait til hsr serviced (=00) to get tcr1 */ tp.tpram[13][4] = tp.tpram[14][6]; /* Set Ref_time = tcr1 */ /* Start 30 ms interrupt clock */ tr.treg[10] = (tr.treg[10] & 0xF3FF); /* Set host sequence = 00 for matches scheduled. This will cause match every ref_time, with ref_ time incremented +30 ms ea. time, automatically */ tr.treg[5] = (tr.treg[5] | 0x2000); /* Set interrupt enable for channel 13 */ tr.treg[12] = 0x0400; /* Set hsr = 01 (host initiated pulse) */ } else if(vp.synch_flag == 1) { /* Clock reset. Set up read time function */ tr.treg[5] = (tr.treg[5] & 0xDFFF); /* disable interrupt */ tr.treg[10] = (tr.treg[10] | 0x0800); /* Set host seq = 10 for read tcr1 only */ vp.synch_flag = 2; vp.curr_clock = 0; vp.clock_time = 0; vp.clk_roltim = 0; tr.treg[12] = 0x0400; /* Set hsr = 01 (host initiated pulse) */ while(tr.treg[12] & 0x0C00); /* Wait til hsr serviced (=00) to get tcr1 */ vp.old_clock = tp.tpram[14][6]; /* Save tcr1 */ vp.old_clock2 = vp.curr_clock; } else { /* synch_flag = 2 */ tr.treg[12] = 0x0400; /* Set hsr = 01 (host initiated pulse) */ while(tr.treg[12] & 0x0C00); /* Wait til hsr serviced (=00) to get tcr1 */ vp.tcr1 = tp.tpram[14][6]; if(vp.tcr1 >= vp.old_clock) vp.elapsed_time = vp.tcr1 - vp.old_clock; else /* Handle rollover. Note: This clock must be called at least every 65 ms to avoid > 1 rollover in tcr1. */ vp.elapsed_time = (0xFFFF - vp.old_clock) + 1 + vp.tcr1; /* Elapsed time since last call */ vp.elapsed_time = ((long)vp.elapsed_time * 160) / CLK_FREQ; vp.curr_clock += vp.elapsed_time; /* curr_clock= microsecs since clock reset */ vp.old_clock = vp.tcr1; /* Calc clock time since clock reset: 32-bit, 0.1 ms increment clock: 0 @ synch & no overflow for > 100 hrs */ if(vp.curr_clock < vp.old_clock2) vp.clk_roltim += 42949672; /* = (2**32)/100 (rollover counter) */ vp.clock_time = vp.clk_roltim + (vp.curr_clock / 100); /* ms x 10; use curr_clock vice accumulating elapsed_time/100, since latter would result in roundoff time lag */ vp.old_clock2 = vp.curr_clock; } /* end of if(synch_flag) tests */ return; } void clock_interrupt(void); asm (" .text .align 2 .globl clock_interrupt clock_interrupt: MOVEML %D0-%D7/%A0-%A6,%SP@- jbsr clock_intrpt2 MOVEML %SP@+,%D0-%D7/%A0-%A6 RTE"); /* Restore registers */ void clock_intrpt2(void) { /* Enter here when interrupt occurs. */ tr.treg[16] = (tr.treg[16] & 0xDFFF); /* Read & clear interrupt status register */ tr.treg[12] = 0x0400; /* Set hsr = 01 (host init plse) to immed restart clock */ vp.clock_time += 300; /* Update clock (ms x 10) */ return; } void qspi_service(void) { if(vp.qspi_svce_flag == 3) goto FPONOFF2; if(vp.qspi_svce_flag == 2) goto STARTAD; /* Turn f/pump on, if off; off, if on */ FPONOFF: qr.qreg[12] |= 0x0100; /* CPHA=1 for Ser Sw */ qr.qreg[14] = 0x2000; /* wren=0, endqp=newqp=0 */ if(vp.fpump_on == 0) qrm.qxmt[0] &= 0xFFFE; /* f/pump on pin 0 of ser sw. This pin is lsb; pins use invrtd logic: 0 = on */ else qrm.qxmt[0] |= 0x0001; qr.qreg[15] &= 0xFF7F; /* clear spif */ qr.qreg[15] &= 0xFEFF; /* clear halt */ qr.qreg[13] |= 0x8000; /* Start qspi: SPE=1 */ do { /* Wait til done (spif=1) */ if((qr.qreg[15] & 0x0080) != 0) { qr.qreg[15] &= 0xFF7F; /* clear spif */ break; } } while(1); /* Since wren=0, qspi automatically stops, sets SPE=0 */ vp.fpump_on = 1 - vp.fpump_on; if(vp.qspi_svce_flag != 3) return; /* Turn on A/D process */ STARTAD: qr.qreg[12] &= 0xFEFF; /* CPHA=0 for A/D */ qr.qreg[14] = 0x6C01; /* wren=1, endqp=12, newqp=1 */ qr.qreg[15] &= 0xFF7F; /* clear spif */ qr.qreg[15] &= 0xFEFF; /* clear halt */ qr.qreg[13] |= 0x8000; /* Start qspi: SPE=1 */ /* Wait for qspi cmds to be sent to all 12 A/D channels (~1 ms) */ do { if((qr.qreg[15] & 0x0080) != 0) /* spif=1 */ break; } while(1); /* Now wait for qspi cmd to be sent to chan 1 again, ensuring all data in qrec[1] to [12] is valid */ do { if((qr.qreg[15] & 0x000F) >= 1) /* cptqp >= 1 */ break; } while(1); return; /* Turn f/pump on/off while a/d running */ FPONOFF2: /* Need to stop qspi to change cpha register */ qr.qreg[15] |= 0x0100; /* first set Halt */ do { /* Wait til halted */ if((qr.qreg[15] & 0x0020) != 0) { /* halta=1 */ break; } } while(1); qr.qreg[13] &= 0x7FFF; /* stop qspi: SPE=0 */ qr.qreg[15] &= 0xFFDF; /* clear halta */ goto FPONOFF; } float randu(void) { /* Generate uniformly distributed random no. bet 0 and 1 */ vp.dseed = ((float)1.6807e04) * vp.dseed; vp.dseed = vp.dseed - (int_2_flt(flt_2_int(vp.dseed / vp.d2p31m),32,0) * vp.d2p31m); /* modulo function */ return(vp.dseed / vp.d2p31); } float int_2_flt(long inpt_int,short int_bits,short int_sign) { /* Converts int to float IAW IEEE. Usage: bits=non-zero bits in inpt_int=32,16 or 8; sign=0 (unsigned) or 1 */ #define BIT31 (unsigned long)0x80000000 if(inpt_int == 0)return((float)0.); vp.fp_ptrf = &vp.fp_flt_out; vp.fp_expn = 32 - int_bits; vp.fp_sign = 0; if(int_sign == 0) /* unsigned */ vp.fp_mantissa = inpt_int; else if(inpt_int >= 0) /* signed, + */ vp.fp_mantissa = inpt_int; else { /* signed, - */ vp.fp_mantissa = -inpt_int; vp.fp_sign = 1; } vp.fp_mantissa <<= vp.fp_expn; for(vp.fp_ix = 0; vp.fp_ix < int_bits; vp.fp_ix++) { if((BIT31 & vp.fp_mantissa) == 0) { vp.fp_mantissa <<= 1; vp.fp_expn++; } else { vp.fp_mantissa <<= 1; vp.fp_expn = (31 - vp.fp_expn) + 127; break; } } vp.fp_mantissa >>= 9; vp.fp_expn <<= 23; *vp.fp_ptrf = vp.fp_mantissa; *vp.fp_ptrf |= vp.fp_expn; if(vp.fp_sign) *vp.fp_ptrf += BIT31; return(vp.fp_flt_out); } long flt_2_int(float inpt_flt) { /* Converts float to long int IAW IEEE. Usage: max float input is +/- 2.1474e09 */ #define EXPN_MASK 0x7F800000 #define MANTISSA_MASK 0x007FFFFF if((inpt_flt > (float)-1.0) && (inpt_flt < (float)1.0)) { return(0); } vp.fp_ptrf = &inpt_flt; vp.fp_expn = ((*vp.fp_ptrf & EXPN_MASK)>>23) - 127; vp.fp_mantissa = (*vp.fp_ptrf & MANTISSA_MASK); vp.fp_ix = 32 - vp.fp_expn - 9; if(vp.fp_ix > 0) vp.fp_mantissa >>= vp.fp_ix; if(vp.fp_ix < 0) vp.fp_mantissa <<= -vp.fp_ix; vp.fp_mantissa += (1 << vp.fp_expn); if(inpt_flt >= (float)0.) vp.fp_int_out = vp.fp_mantissa; else vp.fp_int_out = -vp.fp_mantissa; return(vp.fp_int_out); } void excptn_interrupt(void) { tr.treg[15] = (tr.treg[15] & 0x000F); /* Zero priority for channels 7 to 2 */ tr.treg[14] = (tr.treg[14] & 0xFFC0); /* Zero priority for channels 10 to 8 */ for(vp.exc_ix=2; vp.exc_ix <= 10; vp.exc_ix++) { /* Turn off injectors and spark */ tp.tpram[vp.exc_ix][0] = 0x1AB2; /* channel ctl - psc to force pins low */ } tr.treg[13] = 0xFFF0; /* Set hsr to force mode to turn off channels 7 to 2. */ tr.treg[12] = 0x003F; /* Set hsr to force mode to turn off channels 10 to 8. */ tr.treg[15] = (tr.treg[15] | 0xFFF0); /* Reset priority for channels 7 to 2 */ tr.treg[14] = (tr.treg[14] | 0x003F); /* Reset priority for channels 10 to 8 */ if(vp.fpump_on == 1) { /* If f/pump on, turn off */ vp.qspi_svce_flag = 3; qspi_service(); } vp.check_eng[0] = 7; if(vp.pc_connect == 0) { /* Send sys err msg to lcd */ qr.qreg[5] = (qr.qreg[5] & 0xFF7F); /* Clear xmt interrupt enable(tie=0) */ for(vp.exc_ix = -2; vp.exc_ix < 6; vp.exc_ix++) { if(vp.exc_ix == -2) qr.qreg[7] = 254; /* puts lcd in instr mode */ else if(vp.exc_ix == -1) qr.qreg[7] = 1; /* lcd 'clear' char */ else qr.qreg[7] = cp.lcd_err[vp.exc_ix]; /* Wait for byte to be sent */ do { if((qr.qreg[6] & 0x0100) != 0) /* check sc status reg (tdre) til ready to xmit next byte */ break; } while(1); } /* end 'for' loop - msg sent to lcd */ } else { /* Wait for am't of time slightly > time req'd for full update of pc display (vp) variables (ensures check_eng sent if pc in display mode). */ vp.synch_flag = 1; /* Reset clock */ usr_clock(); vp.tmpint = (1100000 * 8 * NO_VPD_BYTES) / cp.sci_baud_pc; /* us */ do { usr_clock(); if(vp.curr_clock > vp.tmpint) /* check time */ break; } while(1); } /* Go into endless loop */ while(1); }