/************************************************************************* $Archive: /PACS/OnBoard/ChopGrat.s $ $Revision: 1.1 $ $Date: 2005/05/04 15:46:40 $ $Author: pierre $ $Log: ChopGrat.s.bak,v $ Revision 1.1 2005/05/04 15:46:40 pierre version 5.024 ; ; 76 1/05/05 10:19a Pacs Egse ; ; 75 12/01/04 3:46p Pacs Egse ; ; 73 10/26/04 4:08p Pacs ; ; 72 10/15/04 3:40p Pacs ; ; 71 9/01/04 4:30p Pacs Egse ; ; 70 8/26/04 10:36a Pacs Egse ; ; 69 8/20/04 6:24p Pacs Egse ; ; 68 8/16/04 10:56a Pacs Egse ; ; 65 6/10/04 6:35p Pacs Egse ; ; 64 6/07/04 3:30p Pacs Egse ; ; 63 5/28/04 6:39p Pacs Egse ; ; 62 5/21/04 4:11p Pacs Egse ; ; 61 5/19/04 10:14a Pacs Egse ; ; 60 5/17/04 5:36p Pacs Egse ; ; 59 5/14/04 6:31p Pacs Egse ; ; 58 5/11/04 2:49p Pacs Egse ; ; 57 5/05/04 4:33p Pacs Egse ; ; 56 5/03/04 2:07p Pacs Egse ; before reorganizing the chopper PID ; ; 55 4/29/04 5:52p Pacs Egse ; ; 53 4/15/04 3:07p Pacs Egse ; ; 52 4/07/04 2:46p Pacs Egse ; ; 51 4/06/04 2:51p Pacs Egse ; ; 50 3/30/04 3:29p Pacs Egse ; CS code ready to be tested ; ; 49 3/05/04 4:09p Pacs Egse ; ; 48 2/27/04 6:22p Pacs Egse ; implemented FW Spec controller ; ; 47 2/05/04 5:29p Pacs Egse ; After the Grating Launch lock life test ; ; 46 1/14/04 5:00p Pacs Egse ; Finalized code for grating before QM Life test. Some parts (launch ; lock + limit switch are temporary because the hardware (electronic) is ; not yet ready) ; ; 45 12/17/03 5:41p Pacs Egse ; Homing of the grating and redundant grating ; ; 44 10/02/03 2:46p Pacs Egse ; after test on grating on 26/09/03 ; ; 43 7/29/03 4:06p Pacs Egse ; Test of the new Grating HW ; ; 42 6/10/03 12:01p Pacs Egse ; Grating servo code tuning ; ; 41 5/23/03 2:19p Pacs Egse ; ; 40 5/23/03 8:21a Pacs Egse ; Added 2 commands to start the link to SPU + writing to EEPROM ; implemented ; ; 39 3/31/03 3:16p Pacs Egse ; Compiled for MOSAIC_BOARD ; ; 38 3/31/03 2:03p Pacs Egse ; test chopper jmg ; ; 37 3/25/03 3:18p Pacs Egse ; test chopper JMG + MG ; ; 36 2/05/03 6:06p Pacs Egse ; -Lot's of changes to the 1355 drivers : new high level functions : ; StartLink and StopLink ; - Red DEC behaviour copied on Blue DEC ; - Dec Controllers updated to support switch-on/off and re-establish ; communication after ; ; 35 2/05/03 10:32a Pacs Egse ; Version for Fit-check chopper (with proto hk boards) ; ; 34 1/24/03 4:21p Pacs Egse ; test chopper with new HK proto - JMG ; + update of Grating servo - MG ; ; 33 12/18/02 11:20a Pacs Egse ; Included new version of Grating PID controller (Marc Guiot version 3) ; ; 32 12/13/02 10:37a Pacs Egse ; renamed K_BPOS_PID_CTRL_STATUS_MOVING_DOWN1 into ; K_BPOS_PID_CTRL_STATUS_MOVING_DOWN. The variable was not defined (the ; compiler gives it a zero value). This was preventing the grating and ; filter wheels to move down. ; ; 31 10/23/02 4:36p Pacs Egse ; Offsets have been defined to take into account the difference between ; EM and AVM implementation. ; ; 30 9/05/02 3:25p Pacs ; correct status "/* r14 = bset r14 by correct setting of status bit ; K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; ; "disable pid" can abort the trajectory generator ; ; 29 9/04/02 12:00p Pacs ; remove chopper hardware access, change timing FPGA bit positions for ; version 2_5 using constants from mim.h ; ; 28 9/04/02 10:54a Pacs ; intermediate version for chopper fit check : position command copied to ; DAC and position sensor copied to HK ; ; 27 8/28/02 12:20p Pacs ; add output to chopper DAC ; ; 26 8/28/02 12:02p Pacs ; Added the header with the history log *************************************************************************/ #include #include "c:\virtuoso\adi21020\inc\macro.h" #define INCLUDE_DEFINE_ONLY #include "..\OnBoard\pid_ctrl.h" #include "..\OnBoard\mim.h" /* ISR : _user_isr AUTHOR : AMazy with contributions from J-M Gillis N Martin M Guiot USE : In this function, the following tasks are performed : - acquiring the HK analog measurements - acquiring the digital measurements ( hardware status bits and timing FPGA data ) - controlling the chopper - generating the trajectory for the chopper - controlling the Grating - generating the trajectory for the Grating - controlling the filter wheel 1 ( spectrometer ) - controlling the filter wheel 2 ( photometer ) - copying the hardware control/mode bits and the calibration sources commands to the hardware SEE ALSO : "Seq.c" */ #define DEBUG_SIGNALS #define FPGA_NOPS /*nop;nop;nop;*/ /* need some nops before/after access to FPGA registers, $$should be useless once we use the final backplane */ /*************************************************************************************/ /* START OF CODE SEGMENT */ /*************************************************************************************/ .segment /pm seg_pmco; /********************************************************/ /* EXTERNAL VARIABLES FROM VIRTUOSO */ /********************************************************/ .extern _K_ArgsP; /* pointer to argument stack for ISR - Virtuoso convention */ .extern _K_Args; /* argument stack for ISR - Virtuoso convention - not used */ .extern gSemaCS1; /* structure used to signal SEMA_TIMER_CS_1 */ .extern gSemaCS2; /* structure used to signal SEMA_TIMER_CS_2 */ .extern gSemaTempSensores; /* structure used to signal SEMA_TEMP_SENSORS */ .extern gHkIndexErr; /* debug counter of hardware / software index compare fail */ .extern gHkSoftIndex; /* software HK array index to be compared with hardware index */ .extern gCurrentCalSrc; .extern gCalSrc500msPeriodCounter; /************************************************************************************/ /* TEMPORARY VARIABLES ( LOCAL STORAGE ) */ /************************************************************************************/ .extern gHallSensorA; .extern gHallSensorB; #ifdef DEBUG_SIGNALS .extern gHC; #endif /************************************************************************************/ /* EXTERNAL VARIABLES FOR FAILURE DETECTION AND DEBUG */ /************************************************************************************/ .extern gIsrDelayErr; /* counter of "isr too late" errors */ .extern gHkIndexErr; /* counter of hardware / software index compare fail */ .extern gHkSoftIndex; /* software HK array index to be compared with hardware index */ .extern gHkPointer; /* copy of index reg for debug */ .extern gHkDispl; /* copy of displacement for debug */ .extern gSelectControllersMode; .extern gpFPGA; .extern gpBolcFrequencyDivider; .extern gMeasuredTimingResidueLo; .extern gMeasuredTimingResidueHi; /*************************************************************************************/ /* START OF CODE */ /*************************************************************************************/ .global _user_isr; _user_isr: START_ISR; //save MODE1, ASTAT, r0, r1, r2, r4, r8, r12, i4, i12 bit set MODE2 CAFRZ; /* freeze cache for the sake of the interrupted task */ nop; /* nop mandatory after a set system register */ /***************************************/ /* prologue : set up environment */ /***************************************/ /* change to alternate registers sets */ bit set MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; nop; /* nop mandatory after a set system register */ /********************************************************/ /* part 1 : Housekeeping */ /* assumption about environment : */ /* NOTE I REGISTERS 8-15 HAVE ONLY 24 BITS */ /* i15 is reserved as ISR call counter */ /* m15 must contain "1" ( increment ) */ /* i0 must be pointer to grating PID block */ /* i1 must be pointer to FW1 ( spectro ) PID block */ /* i2 must be pointer to FW2 ( photo ) PID block */ /* i3 must be pointer to chopper PID block */ /* i4 must be a pointer to cal src 2 PID block (right now, it is used as a temporary register) */ /* i5 must be a pointer to cal src 1 PID block */ /* i6 must be a pointer to HK array */ /* ( high level software interface ) */ /* "mode" field of software command register and */ /* of MIM hardware command register must select */ /* active mechanism ( grating, fw1 or fw2 ) */ /* These bits will connect the power amplifier to */ /* one of the motors, and the HK multiplexer to the */ /* corresponding pair of Hall sensors */ /* on AVM : bits 0,1 control the HK multiplexer : */ /* 00 = grating */ /* 01 = filter wheel 1 */ /* 10 = filter wheel 2 */ /* 11 = not implemented */ /* on AVM : bits 7,8 control the amplifier : */ /* 00 = grating */ /* 01 = filter wheel 1 */ /* 10 = grating ( not used ) */ /* 11 = filter wheel 2 */ /* NOTE ON HALL SENSORS AND DACS : HI/LO LVL I/F */ /* current implementation is 1 word for 2 x 16 bits */ /* to be changed to 2 words for SENSORS and DACS */ /********************************************************/ /*************************************************************************************/ /* BUILD OUTPUT WORD FOR HARDWARE CONTROL + CAL SRC DACS ONLY VALID FOR AVM HK BOARD */ /*************************************************************************************/ r0 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r4 = 0; r4 = r4 or FDEP r0 by 0:16; #ifdef DEBUG_SIGNALS dm( _gHC ) = r4; /* debug preserve value until end of routine*/ r4 = bset r4 by K_BPOS_MIM_ISR_EXECUTING; /* spare bit for ISR start signal */ #endif dm(MIM_CTRL_REG) = r4; /* write to hardware */ FPGA_NOPS; modify ( i15, 1 ); /* increment ISR activation counter (DMC_IRS_COUNT) */ dm(ISR_COUNT,i6) = i15; /*******************************************************************/ /* ANALOG HK: CHOPPER POS + HALL SENSORS + ANALOG HK */ /*******************************************************************/ /* read from TIMING_MUX area HK digital inputs */ FPGA_NOPS; r2=dm(TIMING_MUX); /* read MIM FPGA status (and index) */ FPGA_NOPS; r15=FEXT r2 BY 19:K_BLEN_MIM_STATUS_HK_INDEX; /* r15 = analog hk index */ //$$TEMP /* =================================================================*/ /* DEBUG JMG 24/3/02 compare hardware counter with software counter */ /* if error count < 0, must init software index and inc err */ r4 = dm(_gHkIndexErr); r4 = pass r4; if GE jump (do_compare); dm(_gHkSoftIndex) = r15; r4= r4 + 1; dm(_gHkIndexErr) = r4; jump(after_compare); /* increment software index and compare with hardware index */ do_compare: r4 = dm(_gHkSoftIndex); r4 = r4 + 1; r5 = 64; /* number of channels */ COMP ( r4, r5); if EQ r4 = r4 - r4; /* short way to code r4 = 0 */ dm(_gHkSoftIndex) = r4; comp (r4, r15); if EQ jump (after_compare); /* increment error count and resync software counter */ r4 = dm(_gHkIndexErr); r4 = r4 + 1; dm(_gHkIndexErr) = r4; dm(_gHkSoftIndex) = r15; after_compare: /* END DEBUG JMG 24/3/02 */ //$$END TEMP FPGA_NOPS; r1=dm(MIM_CHOP_POS_AND_HK); /* read chopper pos (16msb) & HK measurements (16lsb)*/ FPGA_NOPS; read_chopper_pos: r0 = fext r1 by 0:16 (se); /* compute 32 bit signed value of chopper position */ read_analog_hk: m6=r15; /* m6 = analog hk index */ r15=FEXT r1 BY 16:16 (se); /* isolate HK value ( from grating front end / EM electronics */ dm(m6,i6)=r15; /* store HK in array */ read_hall_sensors: FPGA_NOPS; r1=dm(MIM_HALL_SENSORS); /* read Hall sensors */ FPGA_NOPS; r4 = fext r1 by 16:16 (se); /* store Hall A */ dm(_gHallSensorA) = r4; r4 = FDEP r1 BY 0:16 (se); /* store Hall B */ dm(_gHallSensorB) = r4; read_inductosyn: FPGA_NOPS; r3=dm(MIM_INDUCTOSYN); /* read Inductosyn counter */ FPGA_NOPS; /*******************************************************************/ /* WRITE FW-GRATING AND CHOPPER DAC */ /*******************************************************************/ /* write DAC values from each PID block to hardware */ /* note grating and fw share the same DAC and amplifier */ r4 = 0; r5 = dm(GRAT_FW_DAC1, i6); /* grating/fw DACS phase 1 ( 16 bits ) */ dm(MIM_GRAT_FW_DAC1) = r5; FPGA_NOPS; r5 = dm(GRAT_FW_DAC2, i6); /* grating/fw DACS phase 2 ( 16 bits ) */ dm(MIM_GRAT_FW_DAC2) = r5; FPGA_NOPS; dm(MIM_GRAT_FW_DACWRT) = r5; /* mandatory write command to really modify the DAC value */ FPGA_NOPS; r4 = dm(OY, i3); /* chopper DAC = 16 LSB, 16 MSB = don t care */ r5 = dm(TK, i3); /* is chopper in open loop ? */ BTST r5 BY K_BPOS_CHOP_CTRL_STATUS_OPEN_LOOP; if not sz r4 = dm(SP, i3); /* if yes, copy setpoint into output */ FPGA_NOPS; dm(MIM_CHOPPER_DAC) = r4; FPGA_NOPS; dm(MIM_CHOPPER_DACWRT) = r4; /* mandatory write command to really modify the DAC value */ FPGA_NOPS; /***********************************************************************/ /* CALIBRATION SOURCE READ AND WRITE */ /***********************************************************************/ /********************************************************************/ /* get measurement and index before changing output ! */ /* there are 8 measurements in BB */ /* channel 0 = 0V fixed */ /* 1 = -Vref = -5V fixed */ /* 2 = +Vref = +5V fixed */ /* 3 = VDAC */ /* 4 = sensor voltage, gain = 2 */ /* 5 = sensor voltage, gain = 200 */ /* 6 = sensor current ( voltage on 100 ohm ) gain = 2 */ /* 7 = sensor current ( voltage on 100 ohm ) gain = 200 */ /* note new hardware version sum of 256 measurements */ /********************************************************************/ #ifdef NEW_CAL_SRC cs_begin: r5 = dm(TIMING_MUX); /* r5 = read calibration source ADC and TIMING MUX*/ FPGA_NOPS; cs_test_500ms: r4 = FEXT r5 BY K_BPOS_CS_MUXADR : K_BLEN_CS_MUXADR; r6 = 0; comp(r4, r6); /* all these muxadr shall be = 0 every 500ms */ if NE jump cs_test_64ms; /* if not, look for 64ms interval */ cs_every_500ms: r4 = dm(_gCalSrc500msPeriodCounter); /* we must stay 2*500ms on each src */ r4 = r4 + 1; dm(_gCalSrc500msPeriodCounter) = r4; r6 = 2; r7 = r4 and r6; r7 = ASHIFT r7 by 1; dm(_gCurrentCalSrc) = r7; cs_test_64ms: r4 = FEXT r5 BY K_BPOS_CS_MUXADR : 9; r6 = 0; comp(r4, r6); /* all these muxadr shall be = 0 every 64ms */ if NE jump cs_continue; /* if not, look for 64ms interval */ cs_change_latch: /* the latch must be modified every 64ms */ /* here, r6 shall be zero*/ /*fill the analog mux adr*/ /*copy MUXADR12 in ADR0 of the latch*/ btst r5 by K_BPOS_CS_MUXADR12; if sz jump cs_prepare_latch3; r6 = bset r6 by 3; r6 = bset r6 by 7; cs_prepare_latch3: /*copy MUXADR13 in ADR1 of the latch*/ btst r5 by K_BPOS_CS_MUXADR13; if sz jump cs_prepare_latch4; r6 = bset r6 by 2; r6 = bset r6 by 6; cs_prepare_latch4: /*copy MUXADR14 in ADR2 of the latch*/ btst r5 by K_BPOS_CS_MUXADR14; if sz jump cs_prepare_latch5; r6 = bset r6 by 1; r6 = bset r6 by 5; cs_prepare_latch5: r4 = dm(_gCurrentCalSrc); r7 = 0; comp(r4, r7); if EQ jump cs_prepare_latch_for_src1; cs_prepare_latch_for_src2: r6 = bset r6 by 0; jump cs_latch_ok; cs_prepare_latch_for_src1: r6 = bset r6 by 4; cs_latch_ok: dm(_gCalSrcDacLatch) = r6; cs_continue: btst r5 by K_BPOS_CS_MUXADR11; if sz jump cs_write; /*if MUXADR11 == 0, wait for the signal to stabilize (nothing to do) */ /*if MUXADR11 == 1, accumulate */ r9 = FEXT r5 BY K_BPOS_CS_CHANNEL_INDEX:K_BLEN_CS_CHANNEL_INDEX; r4 = dm(_gCurrentCalSrc); r7 = 0; comp(r4, r7); if EQ jump cs_accumulate_on_src1; cs_accumulate_on_src2: i5=dm(_gpCS2Controller); jump cs_accumulate; cs_accumulate_on_src1: i5=dm(_gpCS1Controller); cs_accumulate: r7 = CS_M1; r9 = r9 + r7; m5 = r9; r5 = FEXT r5 BY 0:16 (se); /* accumulate */ r4 = dm ( m5, i5 ); r5 = r5 + r4; dm ( m5, i5 ) = r5; /* i5 is not modified */ cs_write: r6 = dm(_gCalSrcDacLatch); i5=dm(_gpCS1Controller); r5 = dm(CS_OUT, i5); /* read output from CS1 PID */ r4 = dm(CS_DAC_OFFSET, i5); r5 = r5 + r4; /* cancel DAC offset */ dm(MIM_CS1_DAC) = r5; /* write it to DAC */ FPGA_NOPS; i5=dm(_gpCS2Controller); r5 = dm(CS_OUT, i5); /* read output from CS2 PID */ r4 = dm(CS_DAC_OFFSET, i5); r5 = r5 + r4; /* cancel DAC offset */ dm(MIM_CS2_DAC) = r5; /* write it to DAC */ FPGA_NOPS; dm(MIM_CS_LATCH) = r6; /* mandatory write command to really modify the DAC value */ FPGA_NOPS; #else //NEW_CAL_SRC cs_test: r5 = dm(TIMING_MUX); /* r5 = read calibration source ADC and TIMING MUX*/ FPGA_NOPS; r4 = FEXT r5 BY K_BPOS_CS_MUXADR8 : 3; r6 = 0x7; comp(r6, r4); if NE jump cs_prepare_latch; /* check MUXADR11 : if zero, accumulate on source 2; if 1, accumulate on source 1*/ BTST r5 BY K_BPOS_CS_MUXADR11; if sz jump(cs_accumulate_src2); cs_accumulate_src1: i5=dm(_gpCS1Controller); r4 = FEXT r5 BY K_BPOS_CS_CHANNEL_INDEX:K_BLEN_CS_CHANNEL_INDEX; jump(cs_accumulate); cs_accumulate_src2: #ifdef DEBUG_SIGNALS r6 = dm(_gHC); /* hardware control bits from high level */ r4 = 0; r4 = r4 or FDEP r6 by 0:16; r4 = bset r4 by 12; /* spare bit*/ dm(MIM_CTRL_REG) = r4; /* write to hardware */ FPGA_NOPS; r4 = bclr r4 by 12; dm(MIM_CTRL_REG) = r4; /* write to hardware */ FPGA_NOPS; #endif r4 = FEXT r5 BY K_BPOS_CS_CHANNEL_INDEX:K_BLEN_CS_CHANNEL_INDEX; r4 = r4 - 1; r6 = 0x7; r4 = r4 and r6; /* transforms -1 into 7 */ i5=dm(_gpCS2Controller); cs_accumulate: r6 = CS_M1; r4 = r4 + r6; m5 = r4; r5 = FEXT r5 BY 0:16 (se); /* accumulate */ r4 = dm ( m5, i5 ); r5 = r5 + r4; dm ( m5, i5 ) = r5; /* i5 is not modified */ cs_prepare_latch: r5 = dm(TIMING_MUX); FPGA_NOPS; r4 = FEXT r5 BY K_BPOS_CS_MUXADR : 8; r6 = dm(_gCalSrcDacLatch); /*If the counter is 0, change the latch value*/ r7 = 0; comp(r4, r7); if NE jump cs_write; /*select the calibration source*/ btst r5 by K_BPOS_CS_MUXADR11; if sz jump cs_prepare_latch2; r6 = bset r6 by 8; cs_prepare_latch2: /*copy MUXADR12-14A in MUXADR12-14B*/ r6 = LSHIFT r6 by -4; /*fill MUXADR12-14A*/ /*copy MUXADR12 in bit7 of the latch*/ btst r5 by K_BPOS_CS_MUXADR12; if sz jump cs_prepare_latch3; r6 = bset r6 by 7; cs_prepare_latch3: /*copy MUXADR13 in bit6 of the latch*/ btst r5 by K_BPOS_CS_MUXADR13; if sz jump cs_prepare_latch4; r6 = bset r6 by 6; cs_prepare_latch4: /*copy MUXADR14 in bit5 of the latch*/ btst r5 by K_BPOS_CS_MUXADR14; if sz jump cs_write; r6 = bset r6 by 5; cs_write: i5=dm(_gpCS1Controller); r5 = dm(CS_OUT, i5); /* read output from CS1 PID */ r4 = dm(CS_DAC_OFFSET, i5); r5 = r5 + r4; /* cancel DAC offset */ dm(MIM_CS1_DAC) = r5; /* write it to DAC */ FPGA_NOPS; r5 = dm(CS_OUT, i4); /* read output from CS2 PID */ r4 = dm(CS_DAC_OFFSET, i4); r5 = r5 + r4; /* cancel DAC offset */ dm(MIM_CS2_DAC) = r5; /* write it to DAC */ FPGA_NOPS; dm(MIM_CS_LATCH) = r6; /* mandatory write command to really modify the DAC value */ FPGA_NOPS; dm(_gCalSrcDacLatch) = r6; #endif //NEW_CAL_SRC /************************************************************************************/ /* part 2 : PID measurements update and computation */ /* assumption about part 1 : */ /* r0 ( 16 lsb ) = chopper position */ /* r1 = Hall sensors BUT USE gHallSensorA and gHallSensorB */ /* r2 = MIM hardware status register */ /* r3 = grating position ( 24 bits ) */ /* only valid if grating power ON */ /* further registers usage convention */ /* r4 = error = setpoint - position */ /* r5 = delta error */ /* r6 = integral accumulator */ /* r7 = current setpoint */ /* r14 = servo status word ( SW ) */ /* r15 = servo control word ( CW ) */ /* */ /* other PID fields */ /* feed forward term ( only for chopper ) */ /* rate ( increments of setpoint per interval ) */ /* target ( for trajectory generator ) */ /* */ /************************************************************************************/ /********************************************************/ /* part 3 : execute chopper PID ( always ) */ /* execute function "activate" and "deactivate" */ /* execute commutation ( copy OY to OZ ) if active */ /* note : do not destroy following registers : */ /* r0 = chopper position */ /* r1 = free ( used to be Hall sensors ) */ /* r2 = MIM hardware status register */ /* r3 = grating position ( 24 bits ) */ /* r4 ( error ) */ /* r5 ( speed ) */ /* r6 ( Integral Accumulator ) */ /* r7 ( current setpoint ) */ /* r8 ( temporary and frequency ) */ /* r9 ( 1/divisor ) */ /* r10 ( temporary and output limit ) */ /* r11 ( PID Coefficients ) */ /* r12 ( Accumulator Limit ) */ /* r13 ( temporary ) */ /* r14 ( servo status word ) */ /* r15 ( servo command word ) */ /********************************************************/ r7 = dm(_gSelectControllersMode); /* this word will tell you which position to use and which pid algorithm to use */ btst r7 by K_BPOS_CTRL_MODE_CHOPPER; /* test the chopper mode, 0 = nominal, 1 = simulate position */ if not sz jump chopper_simulate_position; r4 = dm(PO, i3); /* r4 = chopper pos offset */ r0 = r0 + r4; /* r0 = chopper pos = chopper pos read + offset */ jump chopper_test_which_pid; chopper_simulate_position: r0 = dm(SP, i3); chopper_test_which_pid: btst r7 by K_BPOS_CTRL_MODE_CHOPPER_SIMPLIFIED; /* test the chopper mode, 0 = mgu, 1 = jmg*/ if not sz jump pid_jmg; pid_mgu: f5 = dm(CHOP_CUR_POS,i6); dm(CHOP_PREV_POS,i6) = f5; /* Pos(t) becomes Pos(t-1) */ dm(MP, i3) = r0; /* chopper current position in PID block */ f0 = float r0; /* float Pos(t)*/ f4 = dm(CH_F_POS_MUL, i3); /* f4 = PosMultiplier */ f0 = f0 * f4; /* f0 = Pos(t) * PosMultiplier */ dm(CHOP_CUR_POS,i6) = f0; /* store Pos(t) */ r7 = dm(SP, i3); /* load and float current setpoint */ f7 = float r7; f4 = dm(CH_F_SP_MUL, i3); /* f4 = SPMultiplier */ f7 = f7 * f4; /* f7 = SetPoint * SPMultiplier */ f4 = f7 - f0; /* f4 = Error(t) */ f1 = dm(CHOP_CUR_ERR,i6); /* Error(t) becomes Error(t-1) */ dm(CHOP_PREV_ERR,i6) = f1; dm(CHOP_CUR_ERR,i6) = f4; /* Store Error(t) */ r15 = dm(CW, i3); /* r15 = servo control word */ r14 = dm(SW, i3); /* r14 = servo status word */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* command to activate PID */ if not sz jump (chopper_activate_pid); btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* command to deactivate PID */ if not sz jump (chopper_deactivate_pid); r0 = dm(OY, i3); /* load output in case PID inactive */ r14 = bclr r14 by 3; /* reset PID executing bit */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* PID active bit */ if sz jump (chopper_idle); r12 = fix f4; /* r12 = Error(t) */ r6 = dm(IA,i3); /* Load Integral Accumulator */ r6 = r6 + r12; /* New Integral Accumulator */ r12 = dm(IL,i3); /* Load Accumulator Limit */ r6 = CLIP r6 by r12; /* Limit Integral Accumulator */ dm(IA,i3) = r6; /* Store new integral accumulator */ f8 = dm(DIVISOR,i6); /* Divisor to scale back KP KI KD */ f8 = recips f8; f11 = dm(CH_F_KP,i3); /* Load proportional gain */ f0 = f11 * f4; /* KP/DIV*Error */ f5 = dm(CH_F_KI,i3); /* Load integral gain */ f6 = float r6; /* float IA */ f6= f5 * f6; /* S(Error) * KI/DIV */ f11 = dm(FREQ,i6); /* Load frequency in Hz */ f13 = recips f11; /* Invert frequency */ f6 = f6 * f13; /* S(error) * KI/DIV * 1/FREQ */ f0 = f0 + f6; /* KP/DIV*Error + S(Error)*KI/DIV*1/FREQ */ f6 = f1 * f11; /* f6= Error(t-1)*FREQ */ f12 = f4 * f11; /* f12 = Error(t)*FREQ */ f5 = f12 - f6; /* f5 = DError*Freq */ #if 0 f1 = dm(CHOP_AVG_DERROR, i6); f12 = 0.; f6 = f12*f1; f6 = f6 + f5; f12 = 1.; f12 = recips f12; f5 = f6 * f12; dm(CHOP_AVG_DERROR, i6) = f5; #endif f6 = dm(CH_F_KD,i3); /* Load differentail gain */ f6 = f6 * f5; /* KD/DIV*DError*Freq */ f0 = f0 + f6; /* KP/DIV*Error + S(Error)*KI/DIV*1/FREQ + KD*DError*FREQ */ r6 = dm(OY, i3); /* I(t-1) */ f6 = float r6; f5 = dm(CH_F_KIC, i3); f6 = f6 * f5; /* KIC/DIV*I(t-1) */ f0 = f0 - f6; f5 = dm(CHOP_CUR_POS, i6); /* f5 = Pos(t) */ f6 = dm(CHOP_PREV_POS, i6); /* f6 = Pos(t-1) */ f5 = f5 - f6; /* f5 = Speed(t) */ f1 = dm(CHOP_AVG_SPEED, i6); /* f1 = AveragedSpeed(t-1) */ f12 = 1.; f6 = f12 * f1; /* 2*AveragedSpeed(t-1) */ f6 = f6 + f5; /* 2*AveragedSpeed(t-1)+Speed(t) */ f12 = 2.; f12 = recips f12; /* Invert f12 */ f5 = f6 * f12; /* f5 = AveragedSpeed(t) = (2*AveragedSpeed(t-1)+Speed(t))/3 */ dm(CHOP_AVG_SPEED, i6) = f5; /* Store AveragedSpeed(t) */ f6 = f5 * f11; /* f6 = AveragedSpeed(t)*Freq */ f11 = dm(CH_F_KF,i3); /* f11 = Kf/DIV */ f6 = f6 * f11; /* f6 = AveragedSpeed(t)*Freq * Kf/DIV */ f0 = f0 - f6; /* f0 = Output(t) */ /* notch filter */ f1 = dm(CHOP_PREV_OUT1,i6); /* Out1(t-1) becomes Out1(t-2) */ dm(CHOP_PREV_PREV_OUT1,i6) = f1; f1 = dm(CHOP_OUT1,i6); /* Out1(t) becomes Out1(t-1) */ dm(CHOP_PREV_OUT1,i6) = f1; dm(CHOP_OUT1,i6) = f0; /* Out1(t) is the output of the first part of the controller */ f1 = dm(CHOP_NOTCH_PREV_OUT,i6); /*NotchOut(t-1) becomes NotchOut(t-2) */ dm(CHOP_NOTCH_PREV_PREV_OUT,i6) = f1; f1 = dm(CHOP_NOTCH_OUT,i6); /*NotchOut(t) becomes NotchOut(t-1) */ dm(CHOP_NOTCH_PREV_OUT,i6) = f1; f12 = dm(CH_F_CHOP_OUT_FACT, i3); f0 = f0 * f12; f12 = dm(CH_F_CHOP_PREV_OUT_FACT, i3); f1 = dm(CHOP_PREV_OUT1,i6); f6 = f12 * f1; f0 = f0 - f6; f12 = dm(CH_F_CHOP_PREV_PREV_OUT_FACT, i3); f1 = dm(CHOP_PREV_PREV_OUT1,i6); f6 = f12 * f1; f0 = f0 + f6; f12 = dm(CH_F_NOTCH_PREV_OUT_FACT, i3); f1 = dm(CHOP_NOTCH_PREV_OUT, i6); f6 = f12 * f1; f0 = f0 + f6; f12 = dm(CH_F_NOTCH_PREV_PREV_OUT_FACT, i3); f1 = dm(CHOP_NOTCH_PREV_PREV_OUT, i6); f6 = f12 * f1; f0 = f0 - f6; dm(CHOP_NOTCH_OUT,i6) = f0; /* end notch filter */ f11 = dm(CH_F_CLG, i3); /* f11 = CLG (Control loop gain) */ f0 = f0 * f11; f11 = dm(FREQ,i6); /* f11 = Load frequency in Hz */ f6 = dm(CH_F_INDUCTANCE, i3); /* f6 = L (Inductance) */ f10 = f6 * f11; /* f10 = L*FREQ */ r6 = dm(OY, i3); /* I(t-1) */ f6 = float r6; f6 = f10 * f6; /* f6 = I(t-1) * L * FREQ */ f0 = f0 + f6; f11 = dm(CH_F_RESISTANCE, i3); /* f11 = Rt (resistor) */ f11 = f11 + f10; /* f11 = Rt + L*FREQ */ f11 = recips f11; f0 = f0 * f11; #if 0 f1 = dm(CHOP_AVG_OUT, i6); /* f1 = AveragedMOutput(t-1) */ f12 = 2.; f6 = f12 * f1; /* 9*AveragedMOutput(t-1) */ f6 = f6 + f0; /* 9*AverageMOutput(t-1)+MOutput(t) */ f12 = 3.; f12 = recips f12; /* Invert f12 */ f0 = f6 * f12; /* f0 = AveragedMOutput(t) = (9*AveragedMOutput(t-1)+MOutput(t))/10 */ dm(CHOP_AVG_OUT, i6) = f0; /* Store AveragedMOutput(t) */ #endif //0 r0 = fix f0; r10 = dm(OL, i3); /* Load Output limit */ r0 = CLIP r0 BY r10; /* Saturate output to 16bits signed */ dm(OY, i3) = r0; /* store result */ r4 = fix f4; dm(ER, i3) = r4; /* store current error */ /* signal the following error if error > error limit */ r6 = dm(EL, i3); /* r6 = error limit */ r11 = ABS r4; /* r11 = ABS(Error(t)) */ comp(r11, r6); if gt r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_FOLLOWING_ERROR; /* set error bit in PidStatus*/ jump (chopper_idle); pid_jmg: r5 = dm(MP, i3); /* old position */ r5 = r0 - r5; /* speed in arbitrary units ( depend on sample rate ) */ r7 = dm(SP, i3); /* load current setpoint */ r4 = r7 - r0; /* compute current error */ dm(MP, i3) = r0; /* chopper current position in PID block */ r15 = dm(CW, i3); /* servo control word */ r14 = dm(SW, i3); /* servo status word */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* command to activate PID */ if not sz jump (chopper_activate_pid); btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* command to deactivate PID */ if not sz jump (chopper_deactivate_pid); r0 = dm(OY, i3); /* load output in case PID inactive */ r14 = bclr r14 by 3; /* reset PID executing bit */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* PID active bit */ if sz jump (chopper_idle); /* compute PID algorithm */ /* r0 contains current position */ /* r1 is free and was used to read Hall sensors */ /* r2, r3 are not free */ /* r4 contains current error */ /* r7 contains current setpoint */ /* result should be a properly scaled and clipped signed integer in r0 */ r6 = dm(KD, i3); /* DEBUG differential gain */ r6 = r6 * r5 (SSI); /* DEBUG speed x Kd */ r6 = ashift r6 by -8; /* DEBUG divide product by 2 exp 8 use Kd = 0x100 for gain of 1 */ r0 = r6; r6 = dm(KP, i3); /* DEBUG proportional gain */ r6 = r6 * r4 (SSI); /* DEBUG error x Kp */ r6 = ashift r6 by -8; /* DEBUG divide product by 2 exp 8 use Kp = 0x100 for gain of 1 */ r0 = r6-r0 ; /* DEBUG prop term - speed term */ r6 = dm(IA, i3); /* DEBUG load integral accumulator */ r6 = r6 + r4; /* DEBUG accumulate error */ r9 = dm(IL, i3); /* DEBUG accumulator limit */ r6 = CLIP r6 BY r9; /* DEBUG saturate accumulator */ dm(IA, i3) = r6; /* DEBUG store integral accumulator */ r5 = dm(KI, i3); /* DEBUG integral gain */ r6 = r6 * r5 (SSI); /* DEBUG accumulator * Ki */ r6 = ashift r6 by -8; /* DEBUG divide product by 2 exp 8 use Ki = 0x100 for gain of 1 */ r0 = r0+r6; /* DEBUG add integral term */ r6 = dm(KF, i3); /* DEBUG feed forward gain */ r6 = r6 * r7 (SSI); /* DEBUG setpoint * Kf */ r6 = ashift r6 by -8; /* DEBUG divide product by 2 exp 8 use Kf = 0x100 for gain of 1 */ r0 = r0+r6; /* DEBUG add feed forward term */ r9 =0x7FFF; /* DEBUG fixed clipping at DAC full range ( 16 bits signed ) */ r0 = CLIP r0 by r9; dm(OY, i3) = r0; /* store result */ dm(ER, i3) = r4; /* store current error */ /* signal the following error if error > error limit */ r6 = dm(EL, i3); /* r6 = error limit */ r11 = ABS r4; /* r11 = ABS(Error(t)) */ comp(r11, r6); if gt r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_FOLLOWING_ERROR; /* set error bit in PidStatus*/ jump (chopper_idle); /********************************************************/ /* CHOPPER PID ACTIVATION HANDLING */ /********************************************************/ chopper_activate_pid: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* reset activate PID command bit */ r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* set PID active status bit */ r0 = 0; /* set output to zero */ jump (chopper_idle); chopper_deactivate_pid: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* reset deactivate PID command bit */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* reset PID active status bit */ r10=0; dm(OY, i3) = r10; /* output = 0 */ dm(IA, i3) = r10; /* accumulator = 0*/ f10=0.; /* clear all history variables*/ dm(CHOP_CUR_POS, i6) = f10; dm(CHOP_PREV_POS, i6) = f10; dm(CHOP_CUR_ERR, i6) = f10; dm(CHOP_PREV_ERR, i6) = f10; dm(CHOP_AVG_SPEED, i6) = f10; dm(CHOP_AVG_OUT, i6) = f10; dm(CHOP_AVG_DERROR, i6) = f10; chopper_idle: /* here r0 = output word in all cases */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_COMMUTATION; /* commutation active control bit */ if sz jump (chopper_idle2); r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_COMMUTATION; chopper_idle2: btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_COMMUTATION; /* commutation active control bit */ if sz jump (chopper_idle3); r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_COMMUTATION; chopper_idle3: btst r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; /* commutation active control bit */ if sz jump (chopper_no_out); /* if not, leave DAC word alone */ chopper_no_out: /* copy back control and status words to PID block */ dm(CW, i3) = r15; dm(SW, i3) = r14; /********************************************************/ /* part 4 : execute grating PID ( if active ) */ /* assumption about part 1-2-3 : */ /* r0 = free ( was used by chopper ) */ /* r1 = free ( was used by chopper ) */ /* r2 = MIM hardware status register */ /* r3 = grating position ( 24 bits ) */ /* only valid if grating power ON */ /* registers used and not saved ( internal usage ): */ /* r4 ( error ) */ /* r5 ( temporary ) */ /* r6 ( temporary ) */ /* r7 ( current setpoint ) */ /* r8 ( DACS calculation ) */ /* r9 ( DACS calculation ) */ /* r10 ( Output limit ) */ /* r11 ( temporary ) */ /* r12 ( 0 ) */ /* r13 ( range then temporary ) */ /* r14 ( servo status word ) */ /* r15 ( servo command word ) */ /* assumption about environment : */ /* high level is responsible of managing */ /* consistently the power ON, mode, PID active */ /* and trajectory control bits */ /* THEREFORE THIS CODE ASSUMES THAT */ /* IF THE PID ACTIVE BIT IS SET THEN */ /* THE HALL SENSORS AND POSITION ARE VALID !!! */ /********************************************************/ r11 = dm(TK, i0); btst r11 by K_BPOS_PID_CTRL_STATUS_POWER_ON; if sz jump grating_end; /**********************************************/ /* GRATING INIT REGISTERS */ /**********************************************/ r12 = 0; /* r12 = 0 */ r13 = dm(GR_RA, i0); /* r13 = the range */ r14 = dm(SW, i0); /* r14 = PidStatus */ r15 = dm(CW, i0); /* r15 = ControlWord */ /* read limit switches and launch lock switches and copy them in PidStatus */ r11 = dm(MIM_STATUS_REG); FPGA_NOPS; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LL_LOCKED; btst r11 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_LOCKED; if not sz r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LL_LOCKED; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LL_UNLOCKED; btst r11 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_UNLOCKED; if not sz r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LL_UNLOCKED; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LIMIT_SWITCH; btst r11 by K_BPOS_MIM_STATUS_GRATING_LIMIT_SWITCH; if not sz r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LIMIT_SWITCH; /**********************************************/ /* GRATING READ POSITION */ /**********************************************/ grating_read_position: r5 = dm(_gSelectControllersMode); /* this word will tell you if you have to replace the position by the setpoint */ btst r5 by K_BPOS_CTRL_MODE_GRATING; /* test the grating mode, 0 = nominal, 1 = simulate position */ if not sz jump grating_simulate_position; r0 = fext r3 by 0:24 (se); /* compute 32 bit signed value of grating position (extract 24bits)*/ dm(GRAT_IND_READ, i6) = r0; r11 = dm(GRAT_CUR_POS, i6); /* Note: in this block of code, GratPos is the readout of the inductosyn*/ dm(GRAT_PREV_POS, i6) = r11; /* GratPos(t) becomes GratPos(t-1) */ dm(GRAT_CUR_POS, i6) = r0; /* store GratPos(t) */ r11 = r0 - r11; r4 = 0x0000FFFF; r11 = fext r11 by 0:16 (se); r0 = dm(GRAT_TEMP_POS, i6); r0 = r0 + r11; dm(GRAT_TEMP_POS, i6) = r0; #if 0 grating_induct_test_overflow: /* the inductosyn period counter in the FPGA seems not to be initialized at zero at startup */ /* so, it might happen that it overflows showing a big step in the position read */ /* to avoid it, we detect the overflows (jump bigger than 15000000) and correct them (store it in a carry bit) */ r4 = dm(GRAT_POS_CARRY, i6); /* r4 = carry bit */ r5 = r4; r11 = r0 - r11; /* r11 = GratPos(t) - GratPos(t-1) */ grating_induct_test_pos_overflow: r7 = -0x00F00000; comp(r11, r7); if le r4 = r4 + 1; /* if ((GratPos(t) - GratPos(t-1)) < - 0x00F00000), increment carry */ grating_induct_test_neg_overflow: r7 = 0x00F00000; comp(r11, r7); if ge r4 = r4 - 1; /* if ((GratPos(t) - GratPos(t-1)) > 0x00F00000), decrement carry */ dm(GRAT_POS_CARRY, i6) = r4; /* store carry */ r7 = 0x01000000; r7 = r7 * r4 (SSI); r0 = r0 + r7; /* grating position = inductosyn readout + carry * 0x01000000 */ comp(r4, r5); /* if we have detected a big jump, no need to check for small ones */ if ne jump grating_induct_jump_compensated; grating_induct_test_pos_jump: /* there are also some 1 period jump that we need to correct */ r4 = dm(GRAT_PER_CAR, i6); /* r4 = carry bit */ r7 = -0x0000F000; comp(r11, r7); if le r4 = r4 + 1; /* if ((GratPos(t) - GratPos(t-1)) < - 0x0000F000), increment carry */ grating_induct_test_neg_jump: r7 = 0x0000F000; comp(r11, r7); if ge r4 = r4 - 1; /* if ((GratPos(t) - GratPos(t-1)) > 0x0000F000), decrement carry */ dm(GRAT_PER_CAR, i6) = r4; /* store carry */ r7 = 0x00010000; r7 = r7 * r4 (SSI); r0 = r0 + r7; /* grating position = inductosyn readout + carry * 0x00010000 */ #endif grating_induct_jump_compensated: /* Check if we have to reverse the sign of the inductosyn readout. When using the redundant inductosyn (which is in the reverse direction), the position will be given by range - pos.*/ r11 = dm(GR_UR, i0); /* read ChangeInductosynSign */ comp(r12, r11); /* compare it to zero (r12) */ if eq jump grating_homing; /* if it is null, don't change the sign of the readout */ r0 = r13-r0; /* position = range - pos read */ jump grating_homing; grating_simulate_position: r0 = dm(SP, i0); /* position = setpoint */ /**********************************************/ /* GRATING HOMING */ /**********************************************/ grating_homing: /*Test if Homing has been ordered*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_HOME; if sz jump grating_homing_test_progress; /*Homing has been ordered, clear the bit in the CW and set it in SW*/ r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_HOME; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_IN_PROGRESS; grating_homing_test_progress: /*Test if Homing is in progress*/ btst r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_IN_PROGRESS; if sz jump grating_homing_end; /*Homing is in progress, test if the move is finished (Error > threshold)*/ r7 = dm(GRAT_ERRORT_0,i6); /* r7 = last error */ r7 = ABS r7; /* get absolute value of error*/ r11= 0x11111; /* r11= 3deg (threshold)*/ comp(r7, r11); if lt jump grating_homing_end; /*Move is finished*/ r11 = 0xFFFF0000; r11 = r0 and r11; btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* test moving up flag */ if not sz jump (grating_homing_was_moving_up); btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* test moving down flag */ if sz jump (grating_homing_end); grating_homing_was_moving_down: dm(PO, i0) = r11; /* offset = current position*/ /*!!! Now, there is an offset!=0 so, next time the pid ctrl loop will execute, it will see a very big error and react accordingly. So, we'd better reset the PID now */ r11 = r0 - r11; dm(SP, i0) = r11; /* set setpoint to current pos (it should be zero)*/ dm(GRAT_ERRORT_0,i6) = r12; /* to avoid a big derivative effect, set last error to zero*/ r13 = 500000; dm(TG, i0) = r13; /* move the grating to a central position */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; r15 = bset r15 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; jump grating_homing_completed; grating_homing_was_moving_up: r11 = r11 - r13; /* offset = current position - range*/ dm(PO, i0) = r11; /*!!! Now, there is an offset!=0 so, next time the pid ctrl loop will execute, it will see a very big error and react accordingly. So, we'd better reset the PID now */ r13 = r0 - r11; dm(SP, i0) = r13; /* set setpoint to current pos (it should be the range)*/ dm(GRAT_ERRORT_0,i6) = r12; /* to avoid a big derivative effect, set last error to zero*/ r13 = 500000; dm(TG, i0) = r13; /* move the grating to a central position */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; r15 = bset r15 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; grating_homing_completed: r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_IN_PROGRESS; /* set/Clear the status bits*/ r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_COMPLETED; dm(SW, i0) = r14; grating_homing_end: /**********************************************/ /* GRATING LAUNCH_LOCK */ /**********************************************/ grating_ll_test_mech_lock_ordered: /*Test if Mechanical launch lock has been ordered*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_MECHANICAL; if sz jump grating_ll_test_mech_unlock_ordered; /*Lock has been ordered, clear the bit in the CW and set it in SW*/ r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_MECHANICAL; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LOCK_MECHANICAL; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LL_MOVING; /*Send current into the lock motor (it will be done at next ISR execution)*/ r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bset r11 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_LOCKED; dm(CONTROL_WORD, i6) = r11; /*Start a counter to make sure not to send current in the motor for more than 40960 period*/ r11 = 40960; dm(GR_CO, i0) = r11; jump grating_ll_test_lock_in_progress; grating_ll_test_mech_unlock_ordered: /*Test if Mechanical launch unlock has been ordered*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_UNLOCK_MECHANICAL; if sz jump grating_ll_test_lock_in_progress; /*Unlock has been ordered, clear the bit in the CW and set it in SW*/ r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_UNLOCK_MECHANICAL; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_UNLOCK_MECHANICAL; r14 = bset r14 by K_BPOS_GRAT_CTRL_STATUS_LL_MOVING; /*Send current into the unlock motor (it will be done at next ISR execution)*/ r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bset r11 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_UNLOCKED; dm(CONTROL_WORD, i6) = r11; /*Start a counter to make sure not to send current in the motor for more than 8192 period*/ r11 = 40960; dm(GR_CO, i0) = r11; jump grating_ll_test_unlock_in_progress; grating_ll_test_lock_in_progress: /*Test if Mechanical launch locking is in progress*/ btst r14 by K_BPOS_GRAT_CTRL_STATUS_LOCK_MECHANICAL; if sz jump grating_ll_test_unlock_in_progress; /*Locking is in progress, now, there are two modes : one where we test the limit switch, the other one where we wait for 8192 period and stop (this is done in both cases)*/ /*Do we have to test the limit switch ?*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_USE_LIMIT_SWITCH; if sz jump grating_ll_lock_test_timeout; /* read limit switch */ btst r14 by K_BPOS_GRAT_CTRL_STATUS_LL_LOCKED; if sz jump grating_ll_lock_test_timeout; /* limit switch is pressed, locking is almost completed: continue to activate the motor for 400 period and stop */ r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_USE_LIMIT_SWITCH; /* stop looking at the limit switch*/ r11 = 1325; /* program the timeout to continue for 400 periods*/ dm(GR_CO, i0) = r11; grating_ll_lock_test_timeout: /* decrement timeout counter */ r11 = dm(GR_CO, i0); r11 = r11 - 1; dm(GR_CO, i0) = r11; comp(r11, r12); if gt jump grating_ll_end; /* if counter > 0, nothing to do */ /* timeout is reached, stop sending current in the motor */ grating_ll_lock_completed: r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LOCK_MECHANICAL; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LL_MOVING; /*stop sending current into the lock motor (it will be done at next ISR execution)*/ r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bclr r11 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_LOCKED; dm(CONTROL_WORD, i6) = r11; jump grating_ll_end; grating_ll_test_unlock_in_progress: /*Test if Mechanical launch unlocking is in progress*/ btst r14 by K_BPOS_GRAT_CTRL_STATUS_UNLOCK_MECHANICAL; if sz jump grating_ll_end; /*Unlocking is in progress, now, there are two modes : one where we test the limit switch, the other one where we wait for 8192 period and stop (this is done in both cases)*/ /*Do we have to test the limit switch ?*/ btst r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_USE_LIMIT_SWITCH; if sz jump grating_ll_unlock_test_timeout; /* read limit switch */ btst r14 by K_BPOS_GRAT_CTRL_STATUS_LL_UNLOCKED; if sz jump grating_ll_unlock_test_timeout; /* limit switch is pressed, unlocking is almost completed: continue to activate the motor for 400 period and stop */ r15 = bclr r15 by K_BPOS_GRAT_CTRL_COMMAND_LOCK_USE_LIMIT_SWITCH; /* stop looking at the limit switch*/ r11 = 1325; /* program the timeout to continue for 400 periods*/ dm(GR_CO, i0) = r11; grating_ll_unlock_test_timeout: /* decrement timeout counter */ r11 = dm(GR_CO, i0); r11 = r11 - 1; dm(GR_CO, i0) = r11; comp(r11, r12); if gt jump grating_ll_end; /* if counter > 0, nothing to do */ /* timeout is reached, stop sending current in the motor */ grating_ll_unlock_completed: r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_UNLOCK_MECHANICAL; r14 = bclr r14 by K_BPOS_GRAT_CTRL_STATUS_LL_MOVING; /*stop sending current into the unlock motor (it will be done at next ISR execution)*/ r11 = dm(CONTROL_WORD, i6); /* hardware control bits from high level */ r11 = bclr r11 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_UNLOCKED; dm(CONTROL_WORD, i6) = r11; grating_ll_end: /**********************************************/ /* GRATING PID */ /**********************************************/ grating_pid: r4 = dm(PO, i0); /* read offset */ r0 = r0-r4; /* and subtract it from current pos */ dm(MP, i0) = r0; /* grating current position in PID block */ btst r14 by K_BPOS_GRAT_CTRL_STATUS_DEGRADED_MODE; /* test if we are in degraded mode */ if not sz jump grating_degraded_move; /* if yes, jump to degraded mode code */ r7 = dm(SP, i0); /* load current setpoint */ r4 = r7 - r0; /* compute current error with correct sign (SP-MP for nom and MP-SP for red) */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* command to activate PID */ if not sz jump (grating_activate_pid); btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* command to deactivate PID */ if not sz jump (grating_deactivate_pid); r0 = dm(OY, i0); /* load output in case PID inactive */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* PID active bit */ if sz jump (grating_idle); /* compute PID algorithm */ /* r0 contains OY */ /* r1 is free */ /* r2, r3 are not free */ /* r4 contains current error */ /* r7 contains current setpoint */ /* r12 contains 0 */ /* result should be a properly scaled and clipped signed integer in r0 */ /* GRATING PID */ r1 = dm(GRAT_ERRORT_0, i6); /* GratError(t) */ dm(GRAT_ERRORT_0+1,i6) = r1; /* becomes GratError(t-1) */ dm(GRAT_ERRORT_0,i6) = r4; /* Store current error */ /* signal the following error if error > error limit */ r6 = dm(EL, i0); /* r6 = error limit */ r11 = ABS r4; /* r11 = ABS(Error(t)) */ comp(r11, r6); if le jump grating_pid_computation; btst r14 by K_BPOS_GRAT_CTRL_STATUS_HOMING_IN_PROGRESS; if not sz jump grating_pid_computation; r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_FOLLOWING_ERROR; /* set error bit in PidStatus*/ jump grating_deactivate_pid; grating_pid_computation: r6 = dm(IA,i0); /* Load Integral Accumulator */ r6 = r6 + r4; /* New Integral Accumulator */ r11 = dm(IL,i0); /* Load Accumulator Limit */ r6 = CLIP r6 by r11; /* Limit Integral Accumulator */ dm(IA,i0) = r6; /* Store new integral accumulator */ f8 = dm(DIVISOR,i6); /* DIVISOR to scale back KP KI KD */ f8 = recips f8; /* Invert DIVISOR */ r11 = dm(KP,i0); /* Load proportional gain (KP)*/ f11 = float r11; /* Float KP */ f4 = float r4; /* Float GratError(t) */ f0 = f11 * f4; /* KP * GratError */ r5 = dm(KI,i0); /* Load integral gain (KI)*/ f5 = float r5; /* Float KI */ f6 = float r6; /* Float IA */ f6= f5 * f6; /* S(GratError) * KI */ f11 = dm(FREQ,i6); /* Load frequency in Hz */ f13 = recips f11; /* Invert frequency */ f6 = f6 * f13; /* S(GratError) * KI * 1/FREQ */ f0 = f0 + f6; /* KP*GratError + S(GratError)*KI*1/FREQ */ f6 = float r1; /* Float GratError(t-1) */ f6 = f6 * f11; /* GratError(t-1)*FREQ*/ f11 = f4 * f11; /* GratError(t)*FREQ*/ f5 = f11 - f6; /* D(GratError)*FREQ */ r6 = dm(KD,i0); /* Load differentail gain (KD)*/ f6 = float r6; /* Float KD */ f6 = f6 * f5; /* KD*D(GratError)*FREQ */ f0 = f0 + f6; /* KP*GratError + S(GratError)*KI*1/FREQ + KD*D(GratError)*FREQ */ f0 = f0 * f8; /* KP/DIVISOR*GratError + S(GratError)*KI/DIVISOR*1/FREQ + KD/DIVISOR*D(GratError)*FREQ */ r0 = fix f0; /* Convert f to r */ r10 = dm(OL, i0); /* Load Output limit */ r8 = r0; /* r8 = output before clipping */ r0 = CLIP r0 BY r10; /* Saturate output to output limit*/ dm(OY, i0) = r0; /* Store result */ r11 = dm(GR_MAX_OUT_LG, i0); /* clip output by 'long duration output limit' */ r11 = CLIP r8 BY r11; comp(r8, r11); /* check if output has been clipped */ if NE jump grat_out_is_max; dm(GR_CUR_STOP, i0) = r12; /* if it has not been clipped, reset the max_output_counter */ grat_out_is_max: r8 = dm(GR_CUR_STOP, i0); /* if it has been clipped, increment the max_output_counter */ r8 = r8 + 1; dm(GR_CUR_STOP, i0) = r8; r11 = dm(GR_NB_STOP, i0); comp(r8, r11); /* if the max_output_counter is big enough, cut the output and disable the controller */ if LT jump grating_scaling; jump grating_deactivate_pid; grating_scaling: r11 = dm(KC, i0); /* Load Scaling */ r0 = r0 * r11 (SSI); /* output = output * scaling */ r0 = ASHIFT r0 by -10; /* normalize scaling */ jump (grating_idle); grating_degraded_move: r13 = dm(GR_DEG_MOVE, i0); comp(r12,r13); if eq jump grating_deg_move_end; /*init some register*/ r13 = 1; /* r13 = 1 */ /* increment the ISR counter and compare it with 'Rate'*/ r10 = dm(GR_DEG_IC, i0); r10 = r10 + r13; r9 = dm(GR_DEG_RT, i0); comp(r10, r9); /* every 'Rate' interrupt, move the grating one step*/ if ge jump grating_deg_move_one_step; dm(GR_DEG_IC, i0) = r10; jump grating_deg_move_end; grating_deg_move_one_step: dm(GR_DEG_IC, i0) = r12; /* reset the ISR counter */ r10 = dm(GR_DEG_IS, i0); /* r10 = CurrentStep */ r9 = dm(GR_DEG_NS, i0); /* r9 = NbStepsForThisMove */ comp(r10, r9); /* if we have already moved the right number of steps, finish the move */ if eq jump grating_deg_move_finished; grating_deg_continue_move: r10 = r10 + r13; /* increment the step counter */ dm(GR_DEG_IS, i0) = r10; /* and save the value */ /*compute the current position in the phase */ r8 = dm(GR_DEG_CURA, i0); /* r8 = CurrentPosInPhaseA */ r9 = dm(GR_DEG_IN, i0); /* r9 = CurrentInc */ r8 = r8 + r9; /*ensure not to go over the sinus array bounds */ r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; dm(GR_DEG_CURA, i0) = r8; /* output in phase A */ i7=_gSinArray; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position) */ r10 = dm(GR_DEG_KC, i0); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(GR_DEG_OUTA, i0) = r10; r10 = - r10; /* invert sign (because amplifier inverts the sign) */ r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC1, i6)= r10; /* write DAC value to MIM block */ /* output in phase B */ r10 = (NB_STEPS_IN_PHASE/4); r8 = r8 - r10; r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position - phase/4) */ r10 = dm(GR_DEG_KC, i0); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(GR_DEG_OUTB, i0) = r10; r10 = - r10; /* invert sign (because amplifier inverts the sign) */ r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC2, i6)= r10; /* write DAC value to MIM block */ jump grating_deg_move_end; grating_deg_move_finished: dm(GR_DEG_MOVE, i0) = r12; /* the move is finished */ grating_deg_move_end: jump grating_no_out; grating_activate_pid: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_PID; /* reset activate PID command bit */ r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* set PID active status bit */ r0 = 0; /* set output to zero */ jump (grating_idle); grating_deactivate_pid: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_PID; /* reset deactivate PID command bit */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; /* reset PID active status bit */ dm(IA,i0) = r12; /* reset accumulator */ dm(GRAT_ERRORT_0,i6) = r12; /* reset error */ dm(GRAT_ERRORT_0+1,i6) = r12; /* reset previous error */ dm(OY, i0) = r12; /* reset output before offset was added */ /* DAC ouput to 0 */ r0 = 0x0; /* 0x0 = 0V */ dm(GRAT_FW_DAC1, i6) = r0; /* reset DACA */ dm(GRAT_FW_DAC2, i6) = r0; /* reset DACB */ grating_idle: r4 = dm(GRAT_ERRORT_0,i6); dm(ER, i0) = r4; /* Store current error */ btst r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_COMMUTATION; /* commutation active control bit */ if sz jump (grating_idle2); r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_ENABLE_COMMUTATION; grating_idle2: btst r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_COMMUTATION; /* commutation active control bit */ if sz jump (grating_idle3); r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_DISABLE_COMMUTATION; grating_idle3: btst r14 by K_BPOS_PID_CTRL_STATUS_COMMUTATION_ENABLED; /* commutation active control bit */ if sz jump (grating_no_out); /* if not, leave DAC word alone */ /* compute output * Hall value = DAC */ r8 = dm(_gHallSensorA); /* r8 = first Hall sensor */ r9 = dm(GR_HSS, i0); r8 = r8 + r9; r8 = r8 * r0 (SSI); /* HallA multiplied by output */ r8 = ASHIFT r8 BY -15; dm(GRAT_FW_DAC1, i6)= r8; /* write DAC value to MIM block */ r1 = dm(_gHallSensorB); /* r1 = second Hall sensor */ r9 = dm(GR_HSS, i0); r1 = r1 + r9; r1 = r1 * r0 (SSI); /* HallB multiplied by output */ r1 = ASHIFT r1 by -15; dm(GRAT_FW_DAC2, i6)= r1; /* write DAC value to MIM block */ grating_no_out: /**********************************************/ /* GRATING SAVE REGISTERS */ /**********************************************/ /* copy back control and status words to PID block */ dm(CW, i0) = r15; dm(SW, i0) = r14; grating_end: /********************************************************/ /* part 5 : execute spectrometer FW PID ( if active ) */ /* assumption about part 1-2-3 : */ /* r0 ( 16 lsb ) = chopper position */ /* r1 = Hall sensors from motor selected by mode */ /* r2 = MIM hardware status register */ /* registers used and not saved ( internal usage ): */ /* r3 ( temporary ) */ /* r4 ( temporary ) */ /* r5 ( temporary ) */ /* r6 ( temporary ) */ /* r7 ( temporary ) */ /* r8 ( temporary ) */ /* r9 ( temporary ) */ /* r10 ( temporary ) */ /* r11 (Task Status) */ /* r12 (0) */ /* r13 (1) */ /* r14 ( temporary ) */ /* r15 ( temporary ) */ /* assumption about environment : */ /* high level is responsible of managing */ /* consistently the power ON, mode, PID active */ /* and trajectory control bits */ /* NOTE THIS IS A MOCK UP PID TO ENABLE ON BOARD SOFTWARE TEST */ /* IN AVM ENVIRONMENT - TO BE REPLACED BY REAL OPEN LOOP CODE */ /********************************************************/ /*test if the fw spec is currently powered on. If not, nothing to do*/ r11 = dm(FW_TK, i1); btst r11 by K_BPOS_PID_CTRL_STATUS_POWER_ON; if sz jump fw_spec_end; #ifdef HK_PROTO_BOARD /*read the limit switches*/ r14 = dm(MIM_STATUS_REG); r15 = -1; fw_spec_read_pos_a: r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; btst r14 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_UNLOCKED; if sz jump fw_spec_read_pos_b; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; r15 = 0; fw_spec_read_pos_b: r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; btst r14 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_LOCKED; if sz jump fw_spec_read_pos_end; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; r15 = 1; #else // we use the new MIM boards r15 = -1; /* r15 will be used to store the position, -1 means 'undefined' */ r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; fw_spec_read_pos_a: r14 = dm(FW_ST, i1); /* read the threshold for the position sensor */ r12 = dm(FW_SPEC_POSA, i6); /* read the Hall sensor at pos A */ comp(r12, r14); /* if the sensor value is bigger than the threshold*/ if lt jump fw_spec_read_pos_b; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; /* set a bit in the status */ r15 = 0; /* and store the current position in r15*/ fw_spec_read_pos_b: r12 = dm(FW_SPEC_POSB, i6); /* read the Hall sensor at pos B */ comp(r12, r14); /* read the Hall sensor at pos A */ if lt jump fw_spec_read_pos_end; /* if the sensor value is bigger than the threshold*/ r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; /* set a bit in the status */ r15 = 1; /* and store the current position in r15*/ #endif //HK_PROTO_BOARD fw_spec_read_pos_end: /*report the position in the CurrentPos HK value*/ dm(FW_CP, i1) = r15; /*test if the fw spec is currently moving. If not, nothing to do*/ btst r11 by K_BPOS_FW_CTRL_STATUS_MOVING; if sz jump fw_spec_end; /*init some register*/ r12 = 0; /* r12 = 0 */ r13 = 1; /* r13 = 1 */ /********************************************/ /* FW SPEC MOVING */ /********************************************/ /* increment the ISR counter and compare it with 'Rate'*/ r10 = dm(FW_IC, i1); r10 = r10 + r13; r9 = dm(FW_RT, i1); comp(r10, r9); /* every 'Rate' interrupt, move the fw one step*/ if ge jump fw_spec_move_one_step; dm(FW_IC, i1) = r10; jump fw_spec_end; fw_spec_move_one_step: dm(FW_IC, i1) = r12; /* reset the ISR counter */ r10 = dm(FW_IS, i1); /* r10 = CurrentStep */ r9 = dm(FW_NS, i1); /* r9 = NbStepsForThisMove */ comp(r10, r9); /* if we have already moved the right number of steps, finish the move */ if eq jump fw_spec_move_finished; /*Do we have to stop when we reach a location ?*/ fw_spec_test_search_a: btst r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; if sz jump fw_spec_test_search_b; btst r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; if sz jump fw_spec_test_search_b; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; jump fw_spec_move_finished; fw_spec_test_search_b: btst r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; if sz jump fw_spec_continue_move; btst r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; if sz jump fw_spec_continue_move; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; jump fw_spec_move_finished; fw_spec_continue_move: r10 = r10 + r13; /* increment the step counter */ dm(FW_IS, i1) = r10; /* and save the value */ /*compute the current position in the phase */ r8 = dm(FW_CURA, i1); /* r8 = CurrentPosInPhaseA */ r9 = dm(FW_IN, i1); /* r9 = CurrentInc */ r8 = r8 + r9; /*ensure not to go over the sinus array bounds */ r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; dm(FW_CURA, i1) = r8; /* output in phase A */ i7=_gSinArray; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position) */ r10 = dm(FW_KC, i1); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(FW_OUTA, i1) = r10; /* r10 = - r10; */ /* invert sign (because amplifier inverts the sign) */ r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC1, i6)= r10; /* write DAC value to MIM block */ /* output in phase B */ r10 = (NB_STEPS_IN_PHASE/4); r8 = r8 - r10; r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position - phase/4) */ r10 = dm(FW_KC, i1); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(FW_OUTB, i1) = r10; /* r10 = - r10; */ /* invert sign (because amplifier inverts the sign) */ r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC2, i6)= r10; /* write DAC value to MIM block */ jump fw_spec_end; fw_spec_move_finished: r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_MOVING; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; r9 = 0x0; /* reset DAC (0V) */ dm(GRAT_FW_DAC1, i6)= r9; /* write DAC value to Grating MIM block */ dm(GRAT_FW_DAC2, i6)= r9; /* write DAC value to Grating MIM block */ jump fw_spec_end; fw_spec_end: dm(FW_TK, i1) = r11; /********************************************************/ /* part 6 : execute photometer FW PID ( if active ) */ /* assumption about part 1-2-3 : */ /* r0 ( 16 lsb ) = chopper position */ /* r1 = Hall sensors from motor selected by mode */ /* r2 = MIM hardware status register */ /* registers used and not saved ( internal usage ): */ /* r3 ( temporary ) */ /* r4 ( error ) */ /* r5 ( delta error ) */ /* r6 ( integral accumulator ) */ /* r7 ( current setpoint ) */ /* r8 ( DACS calculation ) */ /* r9 ( temporary ) */ /* r10 */ /* r11 */ /* r12 */ /* r13 */ /* r14 ( servo status word ) */ /* r15 ( servo command word ) */ /* assumption about environment : */ /* high level is responsible of managing */ /* consistently the power ON, mode, PID active */ /* and trajectory control bits */ /* NOTE THIS IS A MOCK UP PID TO ENABLE ON BOARD SOFTWARE TEST */ /* IN AVM ENVIRONMENT - TO BE REPLACED BY REAL OPEN LOOP CODE */ /* */ /********************************************************/ /*test if the fw photo is currently powered on. If not, nothing to do*/ r11 = dm(FW_TK, i2); btst r11 by K_BPOS_PID_CTRL_STATUS_POWER_ON; if sz jump fw_photo_end; #ifdef HK_PROTO_BOARD /*read the limit switches*/ r14 = dm(MIM_STATUS_REG); r15 = -1; fw_photo_read_pos_a: r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; btst r14 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_UNLOCKED; if sz jump fw_photo_read_pos_b; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; r15 = 0; fw_photo_read_pos_b: r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; btst r14 by K_BPOS_MIM_STATUS_GRATING_LAUNCH_LOCKED; if sz jump fw_photo_read_pos_end; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; r15 = 1; #else // we use the new MIM boards r15 = -1; /* r15 will be used to store the position, -1 means 'undefined' */ r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; fw_photo_read_pos_a: r14 = dm(FW_ST, i2); /* read the threshold for the position sensor */ r12 = dm(FW_PHOTO_POSA, i6); /* read the Hall sensor at pos A */ comp(r12, r14); /* if the sensor value is bigger than the threshold*/ if lt jump fw_photo_read_pos_b; r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; /* set a bit in the status */ r15 = 0; /* and store the current position in r15*/ fw_photo_read_pos_b: r12 = dm(FW_PHOTO_POSB, i6); /* read the Hall sensor at pos B */ comp(r12, r14); /* read the Hall sensor at pos A */ if lt jump fw_photo_read_pos_end; /* if the sensor value is bigger than the threshold*/ r11 = bset r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; /* set a bit in the status */ r15 = 1; /* and store the current position in r15*/ #endif //HK_PROTO_BOARD fw_photo_read_pos_end: /*report the position in the CurrentPos HK value*/ dm(FW_CP, i2) = r15; /*test if the fw photo is currently moving. If not, nothing to do*/ btst r11 by K_BPOS_FW_CTRL_STATUS_MOVING; if sz jump fw_photo_end; /*init some register*/ r12 = 0; /* r12 = 0 */ r13 = 1; /* r13 = 1 */ /********************************************/ /* FW PHOTO MOVING */ /********************************************/ /* increment the ISR counter and compare it with 'Rate'*/ r10 = dm(FW_IC, i2); r10 = r10 + r13; r9 = dm(FW_RT, i2); comp(r10, r9); /* every 'Rate' interrupt, move the fw one step*/ if ge jump fw_photo_move_one_step; dm(FW_IC, i2) = r10; jump fw_photo_end; fw_photo_move_one_step: dm(FW_IC, i2) = r12; /* reset the ISR counter */ r10 = dm(FW_IS, i2); /* r10 = CurrentStep */ r9 = dm(FW_NS, i2); /* r9 = NbStepsForThisMove */ comp(r10, r9); /* if we have already moved the right number of steps, finish the move */ if eq jump fw_photo_move_finished; /*Do we have to stop when we reach a location ?*/ fw_photo_test_search_a: btst r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; if sz jump fw_photo_test_search_b; btst r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_A; if sz jump fw_photo_test_search_b; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; jump fw_photo_move_finished; fw_photo_test_search_b: btst r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; if sz jump fw_photo_continue_move; btst r11 by K_BPOS_FW_CTRL_STATUS_ON_LOCATION_B; if sz jump fw_photo_continue_move; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; jump fw_photo_move_finished; fw_photo_continue_move: r10 = r10 + r13; /* increment the step counter */ dm(FW_IS, i2) = r10; /* and save the value */ /*compute the current position in the phase */ r8 = dm(FW_CURA, i2); /* r8 = CurrentPosInPhaseA */ r9 = dm(FW_IN, i2); /* r9 = CurrentInc */ r8 = r8 + r9; /*ensure not to go over the sinus array bounds */ r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; dm(FW_CURA, i2) = r8; /* output in phase A */ i7=_gSinArray; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position) */ r10 = dm(FW_KC, i2); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(FW_OUTA, i2) = r10; /* r10 = - r10; */ /* invert sign (because amplifier inverts the sign) */ r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC1, i6)= r10; /* write DAC value to MIM block */ /* output in phase B */ r10 = (NB_STEPS_IN_PHASE/4); r8 = r8 - r10; r9 = K_BMASK_NB_STEPS_IN_PHASE; r8 = r8 and r9; m7 = r8; r9 = dm(m7, i7); /* r9 = sin (current position - phase/4) */ r10 = dm(FW_KC, i2); /* r10 = scaling */ r10 = r10*r9(ssi); r10 = ASHIFT r10 BY -15; /* sin was [-32767, 32767], scale the output */ dm(FW_OUTB, i2) = r10; /* r10 = - r10; */ /* invert sign (because amplifier inverts the sign) */ r9 = 0xFFFF; /* clip at 0xffff */ r10 = CLIP r10 by r9; dm(GRAT_FW_DAC2, i6)= r10; /* write DAC value to MIM block */ jump fw_photo_end; fw_photo_move_finished: r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_MOVING; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_A; r11 = bclr r11 by K_BPOS_FW_CTRL_STATUS_SEARCHING_POS_B; r9 = 0x0; /* reset DAC to 0V */ dm(GRAT_FW_DAC1, i6)= r9; /* write DAC value to MIM block */ dm(GRAT_FW_DAC2, i6)= r9; /* write DAC value to MIM block */ jump fw_photo_end; fw_photo_end: dm(FW_TK, i2) = r11; /********************************************************/ /* part 7 : read hardware status and update */ /* status registers to high level software */ /* OBT missing and other timing FPGA status bits */ /* test SYNC flag and signal to high level */ /* + trajectory generators */ /* note registers usage : */ /* r2 = MIM hardware status register */ /* r4 = copy of TIMING FPGA status bits */ /* bit 0 = flag 0 control readback */ /* bit 1 = period count enable readback */ /* bit 2 = master reset ack */ /* bit 3 = OBT missing */ /* bit 4 = flag 0 state ( sync signal ) */ /* bit 5 = OBT error ( bad frequency ) */ /* r5 = period counter calculation */ /* r6 = copy of Isr_Stat ( sync status bits ) */ /* MODIF 29/5/02 BUILDING STATUS_WORD */ /* MODIF 04/09/02 FPGA TIMING V2_5 */ /********************************************************/ FPGA_NOPS; r4 = dm(TIMING_STATUS); /* TIMING FPGA status 0x80040007 for MOSAIC */ FPGA_NOPS; r5 = fext r2 by 0:16; /* take 16 lsb of MIM ( hardware ) status */ r5 = r5 or fdep r4 by 16:16; /* 16 msb of result are 16 lsb of TIMING FPGA status */ dm(STATUS_WORD, i6) = r5; /* write hardware status register for high level */ i7 = dm(_gpBolcFrequencyDivider); r6 = dm(0, i7); i7 = dm(_gpFPGA); m4 = r6; r6 = dm(m4, i7); /*finally, we have the BolcPllMultiplier */ /* $$timing bug in FPGA access */ nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; r6 = dm(TIMING_PERIOD); /* period measurement */ FPGA_NOPS; dm (PERIOD_COUNT, i6) = r6; r6 = dm(ISR_STAT, i6); /* load sync status register */ /* sync bit often missed because of FPGA bug JMG 3/6/02 */ /* btst r2 by 14; temporary debug use an external bit see schematic in logbook */ /* bug corrected in version 4 JMG 21/6/02 */ /* changed from bit 4 to bit 9 in FPGA V2_5 JMG 4/9/02 */ /* this is the shifted sync used for mechanisms */ btst r4 by K_BPOS_TIMING_SYNC_PRESENT; if sz jump(no_sync); got_sync: r5 = dm(READOUT_COUNT, i6); r5 = r5 + 1; /* increment readout count in chopper position */ dm(READOUT_COUNT, i6) = r5; r6 = bset r6 by K_BPOS_MIM_SYNC_STATUS; r6 = bset r6 by K_BPOS_MIM_SYNC_FLAG; btst r4 by K_BPOS_TIMING_COUNT_ENB; /* period count enabled bit */ if sz jump(after_sync); r5 = dm(SYNC_COUNT, i6); r5 = r5 + 1; /* increment sync counter */ dm(SYNC_COUNT, i6) = r5; jump(after_sync); no_sync: r6 = bclr r6 by K_BPOS_MIM_SYNC_STATUS; after_sync: btst r4 by K_BPOS_TIMING_SYNC_INPUT; /* this is the direct sync used for the software phase lock */ if sz jump(end_sync) ; do_pll: /* signal for debug */ #ifdef DEBUG_SIGNALS r5 = dm( _gHC ) ; r5 = bset r5 by K_BPOS_MIM_ISR_EXECUTING; /* was already set at the beginning of the ISR but not reflected in gHC */ dm (MIM_CTRL_REG) = r5; /* write to hardware port on HK proto board */ FPGA_NOPS; #endif //DEBUG_SIGNALS /****************************************/ /* TODO insert here Deltatec formula */ FPGA_NOPS; r5 = dm(TIMING_RES_LO); FPGA_NOPS; dm(_gMeasuredTimingResidueLo) = r5; FPGA_NOPS; r5 = dm(TIMING_RES_HI); FPGA_NOPS; r0 = 0xFF; r5 = r5 and r0; dm(_gMeasuredTimingResidueHi) = r5; end_sync: dm(ISR_STAT, i6) = r6; /* write sync status register for high level */ /* following code should not alter r6 */ /********************************************************/ /* chopper trajectory generator */ /* first look whether we have to sync */ /* ( sync detected is status bit 0 in r6 ) */ /********************************************************/ r0 = dm(SP, i3); /* setpoint */ r1 = dm(RT, i3); /* load rate */ r2 = dm(TG, i3); /* load target */ r14 = dm(SW, i3); /* load chopper status word */ r15 = dm(CW, i3); /* load chopper command word */ /* check status for PID disabled */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; if not sz jump (chopper_enabled); chopper_abort: /* clear "move" bits in r14 and r15 */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; jump (chopper_finished); /* exit, storing r14 and r15 */ chopper_enabled: /* check status for move in progress */ btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* moving up flag */ if not sz jump (chopper_move_up); btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* moving down flag */ if not sz jump (chopper_move_down); /* no current move, check command */ btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* move up flag */ if not sz jump ( chopper_move); btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; /* move down flag */ if sz jump ( chopper_no_move); chopper_move: r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; /* reset status bit */ /* uncomment next instruction to bypass sync flag */ /* jump(chopper_no_sync); */ btst r15 by K_BPOS_PID_CTRL_COMMAND_USE_SYNCHRO; if sz jump (chopper_no_sync); /* if chopper must move but does not need sync */ btst r6 by K_BPOS_MIM_SYNC_STATUS; if sz jump (chopper_no_move); /* if needs sync but no sync then wait for sync */ r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; /* say sync found */ chopper_no_sync: /* start moving, reset readout count in chopper position */ r5 = r5-r5; dm(READOUT_COUNT, i6) = r5; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_USE_SYNCHRO; /* reset sync bit in command */ btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* which direction ? */ if sz jump (chopper_move_down); chopper_move_up: r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* set move up flag in status word */ r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* and clear in command word */ r0 = r0 + r1; /* increment setpoint */ comp (r0, r2); /* over target */ if lt jump (chopper_still_moving); /* setpoint less than target */ dm(SP, i3) = r2; /* set setpoint to target */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* reset move up flag in status word */ jump (chopper_finished); chopper_move_down: r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* set move down flag in status word */ r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; /* and clear in command word */ r0 = r0 - r1; /* decrement setpoint */ comp (r2, r0); /* over target */ if lt jump(chopper_still_moving); /* setpoint less than target */ dm(SP, i3) = r2; /* set setpoint to target */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* reset move down flag in status word */ jump(chopper_finished); chopper_still_moving: dm(SP, i3) = r0; /* set setpoint to incremented setpoint */ chopper_finished: dm(CW, i3) = r15; /* save chopper command word */ dm(SW, i3) = r14; /* save chopper status word */ chopper_no_move: /********************************************************/ /* grating trajectory generator */ /* first look whether we have to sync */ /* sync detected is status bit 0 in r6 */ /********************************************************/ r0 = dm(SP, i0); /* setpoint */ r1 = dm(RT, i0); /* rate */ r2 = dm(TG, i0); /* target */ r14 = dm(SW, i0); /* load grating status word */ r15 = dm(CW, i0); /* load grating command word */ /* check status for PID disabled */ btst r14 by K_BPOS_PID_CTRL_STATUS_PID_ENABLED; if not sz jump (grating_enabled); grating_abort: /* clear "move" bits in r14 and r15 */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; jump (grating_finished); /* exit, storing r14 and r15 */ grating_enabled: /* check status for move in progress */ btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* moving up flag */ if not sz jump (grating_move_up); btst r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* moving down flag */ if not sz jump (grating_move_down); /* no current move, check command */ btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* move up flag */ if not sz jump ( grating_move); btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; /* move down flag */ if sz jump ( grating_no_move); grating_move: r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; /* reset status bit */ /* uncomment next instruction to bypass sync flag */ /* jump(grating_no_sync);*/ btst r15 by K_BPOS_PID_CTRL_COMMAND_USE_SYNCHRO; if sz jump (grating_no_sync); /* if grating does not need sync */ btst r6 by K_BPOS_MIM_SYNC_STATUS; if sz jump (grating_no_move); /* if needs sync but no sync */ r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_USING_SYNCHRO; /* say sync found */ grating_no_sync: r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_USE_SYNCHRO; /* reset sync bit in command */ btst r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* which direction ? */ if sz jump (grating_move_down); grating_move_up: r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* set move up flag in status word */ r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_UP; /* and clear in command word */ r0 = r0 + r1; /* increment setpoint */ comp (r0, r2); /* over target */ if lt jump (grating_still_moving); /* setpoint less than target */ dm(SP, i0) = r2; /* set setpoint to target */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_UP; /* reset move up flag in status word */ jump (grating_finished); grating_move_down: r14 = bset r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* set move down flag in status word */ r15 = bclr r15 by K_BPOS_PID_CTRL_COMMAND_MOVE_DOWN; /* and clear in command word */ r0 = r0 - r1; /* decrement setpoint */ comp (r2, r0); /* over target */ if lt jump (grating_still_moving); /* setpoint less than target */ dm(SP, i0) = r2; /* set setpoint to target */ r14 = bclr r14 by K_BPOS_PID_CTRL_STATUS_MOVING_DOWN; /* reset move down flag in status word */ jump (grating_finished); grating_still_moving: dm(SP, i0) = r0; /* set setpoint to incremented setpoint */ grating_finished: dm(CW, i0) = r15; /* save grating command word */ dm(SW, i0) = r14; /* save grating status word */ grating_no_move: /**********************************************/ /* epilogue : signal end, restore context */ /**********************************************/ #ifdef DEBUG_SIGNALS r2 = dm (_gHC); /* hardware control bits from high level were saved */ r2 = bclr r2 by K_BPOS_MIM_ISR_EXECUTING; /* spare bit for ISR end signal */ dm(MIM_CTRL_REG) = r2; /* write to hardware */ FPGA_NOPS; #ifdef HK_PROTO_BOARD dm (MIM_CTRL_REG_COPY) = r2; #endif /*HK_PROTO_BOARD*/ #endif //DEBUG_SIGNALS /********************************************************************/ /* DEBUG JMG 24/3/02 test SAMPLE status , if 0 ISR is too late */ /* implemented in FPGA TIMING starting from version 2_5 4/9/02 */ /********************************************************************/ btst r4 by K_BPOS_TIMING_SAMPLE_TIME; /* sample status now in FPGA status */ if not sz jump (after_sample); /* increment error count */ r4 = dm(_gIsrDelayErr); r4 = r4 + 1; dm(_gIsrDelayErr) = r4; after_sample: /* END DEBUG JMG 24/3/02 /*******************************************************************/ /*********************************************************************/ /* TEMPERATURE SENSORS BLOCK */ /*********************************************************************/ FPGA_NOPS; r5 = dm(TIMING_MUX); FPGA_NOPS; r4 = FEXT r5 BY K_BPOS_CS_MUXADR : K_BLEN_CS_MUXADR; /*The TempSensors task must be executed only at 2 Hz, at the beginning of a measure cycle (driven by FPGA) */ /*the mux address is equal to 1 every 500ms, we test it to generate the 2 Hz*/ /*If the counter is 1, we must trigger the tempSensors task*/ r5 = 1; comp(r4, r5); if EQ jump temp_sensors_signal; /*********************************************************************/ /* CALIBRATION SOURCE BLOCK */ /*********************************************************************/ #ifdef NEW_CAL_SRC /*The CS1 task must be executed only at 2 Hz, at the beginning of a measure cycle (driven by FPGA) */ /*the mux address is equal to zero every 500ms, we test it to generate the 2 Hz*/ /*If the counter is 2, this is the beginning of a measure cycle for CS1*/ r5 = 2; comp(r4, r5); if EQ jump cs1_signal; /*If the counter is 3, this is the beginning of a measure cycle for CS2*/ r5 = 3; comp(r4, r5); if EQ jump cs2_signal; jump exit_chop_grat; /* no semaphore to signal, exit the ISR */ cs1_signal: r5 = K_CS_SIGNAL_COPY_MEASURE + K_CS_SIGNAL_CHANGE_OUTPUT_SIGN; i5=dm(_gpCS1Controller); dm(CS_ST, i5) = r5; bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*restore primary registers now because we are using i4 and r2 to signal the semaphore*/ nop; /* nop mandatory after a set system register */ /*signal the semaphore to trigger the CS Controller task*/ i4 = dm(_K_ArgsP); r2 = _gSemaCS1; PRHI_STACK_PSH; jump exit_chop_grat_final; cs2_signal: r5 = K_CS_SIGNAL_COPY_MEASURE + K_CS_SIGNAL_CHANGE_OUTPUT_SIGN; dm(CS_ST, i4) = r5; bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*restore primary registers now because we are using i4 and r2 to signal the semaphore*/ nop; /* nop mandatory after a set system register */ /*signal the semaphore to trigger the CS Controller task*/ i4 = dm(_K_ArgsP); r2 = _gSemaCS2; PRHI_STACK_PSH; jump exit_chop_grat_final; #else //NEW_CAL_SRC /*The CS1 task must be executed only at 2 Hz, at the beginning of a measure cycle (driven by FPGA) */ /*the mux address is equal to zero every 500ms, we test it to generate the 2 Hz*/ /*If the counter is 0, this is the beginning of a measure cycle for CS1*/ r5 = 0; comp(r4, r5); if EQ jump cs1_signal; /*If the counter is 256, this is the beginning of a measure cycle for CS2*/ r5 = 256; comp(r4, r5); if EQ jump cs2_signal; jump exit_chop_grat; /* no semaphore to signal, exit the ISR */ cs1_signal: r5 = K_CS_SIGNAL_COPY_MEASURE + K_CS_SIGNAL_CHANGE_OUTPUT_SIGN; i5=dm(_gpCS1Controller); dm(CS_ST, i5) = r5; bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*restore primary registers now because we are using i4 and r2 to signal the semaphore*/ nop; /* nop mandatory after a set system register */ /*signal the semaphore to trigger the CS Controller task*/ i4 = dm(_K_ArgsP); r2 = _gSemaCS1; PRHI_STACK_PSH; jump exit_chop_grat_final; cs2_signal: r5 = K_CS_SIGNAL_COPY_MEASURE + K_CS_SIGNAL_CHANGE_OUTPUT_SIGN; dm(CS_ST, i4) = r5; bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*restore primary registers now because we are using i4 and r2 to signal the semaphore*/ nop; /* nop mandatory after a set system register */ /*signal the semaphore to trigger the CS Controller task*/ i4 = dm(_K_ArgsP); r2 = _gSemaCS2; PRHI_STACK_PSH; jump exit_chop_grat_final; #endif //NEW_CAL_SRC temp_sensors_signal: bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*restore primary registers now because we are using i4 and r2 to signal the semaphore*/ nop; /* nop mandatory after a set system register */ /*signal the semaphore to trigger the TempSensors task*/ i4 = dm(_K_ArgsP); r2 = _gSemaTempSensors; PRHI_STACK_PSH; jump exit_chop_grat_final; exit_chop_grat: bit clr MODE1 SRCU|SRD1H|SRD1L|SRD2H|SRD2L|SRRFH|SRRFL; /*resotre primary registers*/ nop; /* nop mandatory after a set system register */ exit_chop_grat_final: bit clr MODE2 CAFRZ; /* free cache before return */ nop; /* nop mandatory after a set system register */ ENDISR1; .endseg;