说明：下面程序取自IFX 8位机无传感器PMSM电机矢量控制程序。
整个程序是连续的矢量控制计算函数，其中有图片说明打断，便于更好的理解。
其中包括坐标系变换,磁链角估算，PI速度环电流环调节。
（单片机XC886，Keil编译器Cavin整理）坐标系变换说明：双电阻采样得到两相电流(ia, ib)，由abc120°静止坐标系Clarke变换到直角坐标系(iα, iβ)，由(iα, iβ)静止直角坐标系Park变换到直角旋转坐标系(iq, id)。
直流id不变，通过PI速度环电流环得到期望直流iq，进行限幅控制。
由旋转坐标系(vq, vd)经过Park逆变换到静止坐标系(vα, vβ)，然后再经过矢量调制成PWM控制电机。
无传感器角度估算：由Clarke变换得到(iα, iβ)和由Park逆变换得到的(vα, vβ)，经过低通滤波器PT1，再由直角坐标系变极坐标系得到磁链估算角无传感器开环启动策略：在定子中加入幅值及频率都受控的电流，若PLL收敛,切换到FOC闭环控制。
软件流程图：void FOC_Calculation (void) using 1{#pragma asm;**************************************;* FOC_Calculation;**************************************;*;* this function contains all calculations;* necessary for the field oriented control.;*;* register bank 1 is used;*;**************************************;* push registers;**************************************push ACCpush bpush dphpush dplpush PSWpush SYSCON0; use register bank 0x01mov PSW,#0x08;;anl SYSCON0,#0xFE ; use standard SFRsmov CCU6_PAGE,#0xA0 ; select CCU6-page 0 SST2;**************************************;* ResultRegister3 Control;**************************************;* Multiplex RESR3 for ADC channel;* ch0, ch1 and ch6 in sequential queue;* Add ADC channel ch3 in triggered mode;*;* input ADC_RESR3;* guc_adc_ch;* gui_adc_ch_val;*;* output ADC_QINR0 = {ch0 -> ch1 -> ch6};* gui_adc_ch_val[guc_adc_ch++] = ADC_RESR3;*mov ADC_PAGE,#0x86 ; switch to ADC_page 6 save ST0mov ADC_QINR0,#0x83 ; channel 3, no refill, external trigger//mov ADC_QINR0,#0x07 ; add channel7 to queue (I_pfc) mov ADC_QINR0,#0x00mov r0,#gui_adc_ch_valmov ADC_PAGE,#0x03 ; switch to ADC_page 3 save no ST0mov @r0,ADC_RESRA3H ; gui_adc_ch_val[guc_adc_ch] = RESRA3 (H)inc r0mov a,ADC_RESRA3Lanl a,#0xe0 ; mask ADC status informationmov @r0,a ; gui_adc_ch_val[guc_adc_ch] = RESRA3 (L)//mov gi_I_pfc,ADC_RESRA2H//mov a,ADC_RESRA2L//anl a,#0xe0 ; mask ADC status information//mov gi_I_pfc+1,a;************************************** ;* current_calculation;************************************** ;* Take ADC values from ADC result;* registers and multiply by 8;*;* input ADC_RESR0, ADC_RESR1;*;* output R2 = [ -I2_shunt * 8 ]H;* R3 = [ -I2_shunt * 8 ]L;* A = [ (I2_shunt - I1_shunt) * 8 ]H;* R5 = [ (I2_shunt - I1_shunt) * 8 ]L;* R6 = [ I1_shunt * 8 ]H;* R7 = [ I1_shunt * 8 ]L;*mov r5,ADC_RESRA1Hmov a,ADC_RESRA1Lanl a,#0xE0 ; mask ADC status informationmov r4,amov r7,ADC_RESRA0Hmov a,ADC_RESRA0Lanl a,#0xE0 ; mask ADC status informationmov r6,amov ADC_PAGE,#0xC2 ; switch to old page restore ST0orl SYSCON0,#1 ; switch to mapped SFRsmov MDU_MD0,r6 ; MD3 = ADCR2_H [ RES9 | RES8 | RES7 | RES6 | RES5 | RES4 | RES3 | RES2 ] mov MDU_MD1,r7 ; MD2 = ADCR2_L [ RES1 | RES0 | 0 | VF | DRC | CHNR2 | CHNR1 | CHNR0 ] mov MDU_MD2,r4 ; MD1 = ADCR3_H [ RES9 | RES8 | RES7 | RES6 | RES5 | RES4 | RES3 | RES2 ] mov MDU_MD3,r5 ; MD0 = ADCR3_L [ RES1 | RES0 | 0 | VF | DRC | CHNR2 | CHNR1 | CHNR0 ] mov MDU_MD4,#0x22 ; shift right ADCR2/ADCR3 by 2 bitsmov MDU_MDUCON,#0x13; ; start bit-shiftnop ; 4 MDU readyclr cmov a, MDU_MR0subb a, offsetLmov a, MDU_MR1subb a, offsetHmov r6, ajnc okclr amov R7, amov R6, aok:clr cmov a, MDU_MR2subb a, offsetLmov r5, amov a, MDU_MR3subb a, offsetHmov r4, ajnc ok1clr amov R5, amov R4, aok1://mov r7,MDU_MR0//mov r6,MDU_MR1//mov r5,MDU_MR2//mov r4,MDU_MR3 ;-----------clr csubb a,r5mov r3,aclr asubb a,r4mov r2,a ; R2/R3 = -I2_shuntclr cmov a,r5 ; A = I2_shunt_Lsubb a,r7mov r5,a ; R5 = I2_shunt_L - I1_shunt_L mov a,r4 ; A = I2_shunt_Hsubb a,r6 ; A/R5 = I2_shunt - I1_shunt;**************************************;* phase current;**************************************;* Calculate currents I_phaseA and I_phaseB;* according the sector of space vector;*;* input R2 = [ -I2_shunt * 8 ]H;* R3 = [ -I2_shunt * 8 ]L;* A = [ (I2_shunt - I1_shunt) * 8 ]H;* R5 = [ (I2_shunt - I1_shunt) * 8 ]L;* R6 = [ I1_shunt * 8 ]H;* R7 = [ I1_shunt * 8 ]L;*;* output R4 = [ gi_I_phaseB ]H;* R5 = [ gi_I_phaseB ]L;* R6 = [ gi_I_phaseA ]H;* R7 = [ gi_I_phaseA ]L;*djnz guc_sector_s,SV_sectorB ; Sector A I1_shunt = Iu , I2_shunt = -Iwmov gi_I_phaseA+1,r7mov gi_I_phaseA,r6 ; I_phaseA = I1_shuntmov gi_I_phaseB+1,r5mov gi_I_phaseB,a ; I_phaseB = -(Iw + Iu) = I2_shunt - I1_shunt sjmp phase_current_endSV_sectorB:djnz guc_sector_s,SV_sectorC ; Sector B I1_shunt = Iv , I2_shunt = -Iwmov gi_I_phaseB+1,r7mov gi_I_phaseB,r6 ; I_phaseB = I1_shuntmov gi_I_phaseA+1,r5mov gi_I_phaseA,a ; I_phaseA = -(Iv + Iw) = I2_shunt - I1_shunt sjmp phase_current_endSV_sectorC:djnz guc_sector_s,SV_sectorD ; Sector C I1_shunt = Iv , I2_shunt = -Iumov gi_I_phaseB+1,r7mov gi_I_phaseB,r6 ; I_phaseB = I1_shuntmov gi_I_phaseA+1,r3mov gi_I_phaseA,r2 ; I_phaseA = -I2_shuntsjmp phase_current_endSV_sectorD:djnz guc_sector_s,SV_sectorE ; Sector D I1_shunt = Iw , I2_shunt = -Iu mov gi_I_phaseA+1,r3mov gi_I_phaseA,r2 ; I_phaseA = -I2_shuntmov gi_I_phaseB+1,r5mov gi_I_phaseB,a ; I_phaseB = -(Iu + Iw) = I2_shunt - I1_shunt sjmp phase_current_endSV_sectorE:djnz guc_sector_s,SV_sectorF ; Sector E I1_shunt = Iw , I2_shunt = -Iv mov gi_I_phaseB+1,r3mov gi_I_phaseB,r2 ; I_phaseB = -I2_shuntmov gi_I_phaseA+1,r5mov gi_I_phaseA,a ; I_phaseA = -(Iv + Iw) = I2_shunt - I1_shunt sjmp phase_current_endSV_sectorF:mov gi_I_phaseA+1,r7 ; Sector F I1_shunt = Iu , I2_shunt = -Iv mov gi_I_phaseA,r6 ; I_phaseA = I1_shuntmov gi_I_phaseB+1,r3mov gi_I_phaseB,r2 ; I_phaseB = -I2_shuntphase_current_end:;**************************************;* clark transform;**************************************;* transform from three phase system;* to two phase system;* I_beta = [(I_phaseB*2 + I_phaseA)/(sqr(3)*2)]*2;*;* input gi_I_phaseB;* gi_I_phaseA;*;* output gi_I_alpha;* gi_I_beta;*;I_phaseB*2 + I_phaseAmov a,gi_I_phaseB+1add a,ACCxch a,gi_I_phaseBaddc a,ACCxch a,gi_I_phaseB ; I_phaseB = I_phaseB*2add a,gi_I_phaseA+1mov MDU_MD0,a ; MD0 = [I_phaseB*2 + I_phaseA]_L mov a,gi_I_phaseBaddc a,gi_I_phaseAmov MDU_MD1,a ; MD1 = [I_phaseB*2 + I_phaseA]_H;(I_phaseB*2 + I_phaseA)/(sqrt(3)*2)mov MDU_MD4,#0xE6 ; MD4 = [0.57735 * 2^15]_Lmov MDU_MD5,#0x49 ; MD5 = [0.57735 * 2^15]_Hmov MDU_MDUCON,#0x14; ;->MDU start;I_phaseA*2mov a, gi_I_phaseA+1add a, ACCxch a, gi_I_phaseAaddc a, ACCmov b, axch a, gi_I_phaseA+1mov gi_I_alpha+1, a ;oscilloscopemov gi_I_alpha, b;(I_phaseB*2 + I_phaseA)/(sqrt(3)*2) * 2mov MDU_MD2,MDU_MR2mov MDU_MD3,MDU_MR3mov MDU_MD4,#0x02 ; shift right ADCR2/ADCR3 by 2 bitsmov MDU_MDUCON,#0x13; ; start bit-shiftnopnopmov gi_I_beta+1,MDU_MR2 ; I_beta = (I_phaseB*2 + I_phaseA)/sqr(3) , I_alpha = I_phaseA*2 mov gi_I_beta,MDU_MR3;**************************************;* flux estimation;**************************************;*;* Calculate the flux and angle;* PSI_alpha = integral( V_alpha + I_alpha * STATOR_R ) dt + I_alpha * STATOR_L;* PSI_beta = integral( V_beta + I_beta * STATOR_R ) dt + I_beta * STATOR_L ;* phi = atan( PSI_alpha / PSI_beta );*;* input gi_V_alpha;* gi_I_alpha;* gi_V_beta;* gi_I_beta;* gi_angle;* gi2_Integrator_alpha[0,1,2,3] ; stored values from integration via PT1 ;* gi2_Integrator_beta[0,1,2,3] ; stored values from integration via PT1 ;*;* output gi2_Integrator_alpha[0,1,2,3];* gi2_Integrator_beta[0,1,2,3];* gi_Flux_alpha;* gi_Angle_mem;* gi_angle;*;**************************************;*clr amov CD_STATC,a ; clear keep bits - interrupt disabledmov CD_CON,#0x48 ; Linear rotation mode - auto start MPS = 1 mov CD_CORDYH,gi_V_alpha ; Y = Imov CD_CORDYL,gi_V_alpha+1 ;mov CD_CORDZH,gi_I_alpha ; Z = R I_max = +4096/-4096mov CD_CORDZL,gi_I_alpha+1 ;mov CD_CORDXH,#STATOR_R_H ; X = Vmov CD_CORDXL,#STATOR_R_L ; CORDIC starts automatically; Y = Y + X*Z*16;**************************************;* Calculation ( (I_alpha * 2^STATOR_L_SCALE)* STATOR_L );*mov MDU_MD0,gi_I_alpha+1 ;4mov MDU_MD1,gi_I_alpha ;8#if STATOR_L_SCALE > 0mov MDU_MD4,#STATOR_L_SCALE ;12 leftshiftmov MDU_MDUCON,#0x13; ;16 bitshift#endifmov MDU_MD4,#STATOR_L_L ;12/4mov MDU_MD5,#STATOR_L_H ;16/8 MDU ready (bitshift)#if STATOR_L_SCALE > 0mov MDU_MD0,MDU_MR0 ;20mov MDU_MD1,MDU_MR1 ;24#endifmov MDU_MDUCON,#0x14; ;28 MR = I * Lmov r0,#gi2_Integrator_alpha ;32/2mov MDU_MD4,#FLUX_PT1_Z1_L ;34/6mov MDU_MD5,#FLUX_PT1_Z1_H ;36/10clr F0 ;40/12 F0 = 0 for PT1 scalermov CD_CORDYL,gi_V_beta+1 ;42/16 MDU readymov r5,MDU_MR3 ;40 R5 = [I_alpha * STATOR_L]_Hmov r4,MDU_MR2 ;44 R4 = [I_alpha * STATOR_L]_L CORDIC readymov r7,CD_CORDYH ; R7 = [V_alpha + I_alpha * STATOR_R]_Hmov r6,CD_CORDYL ; R6 = [V_alpha + I_alpha * STATOR_R]_L;**************************************;* Calculation (V_beta + I_beta * STATOR_R);*mov CD_STATC,a ; clear keep bits - interrupt disabledmov CD_CORDYH,gi_V_beta ;mov CD_CORDZH,gi_I_beta ; Z = Imov CD_CORDZL,gi_I_beta+1 ; X = Rmov CD_CON,#0x59 ; Linear rotation mode , start / MPS = 1;**************************************;* integration by PT1 integral(r6,r7)dt;*mov r3,#FLUX_PT1_Z2_Hmov r2,#FLUX_PT1_Z2_Llcall PT1_MDU ; PT1-return value L = *R1 , H = *R0;**************************************;* Calculation (I_beta * STATOR_L);*mov MDU_MD0,gi_I_beta+1mov MDU_MD1,gi_I_beta#if STATOR_L_SCALE > 0mov MDU_MDUCON,#0x13; ; bitshift#endifmov MDU_MD4,#STATOR_L_L ;4mov MDU_MD5,#STATOR_L_H ;8 MDU ready (bitshift)#if STATOR_L_SCALE > 0mov MDU_MD0,MDU_MR0mov MDU_MD1,MDU_MR1#endifmov MDU_MDUCON,#0x14; ; MR = I * L;**************************************;* _gi3_Flux_alpha[4,5] = [R5 + *R0, R4 + *R1];*mov a,r4 ;2 R4 = [ I_alpha * STATOR_L ]_Ladd a,@r0 ;4 *R1 = [ integral(V_alpha + I_alpha * STATOR_R)dt ]_L mov gi_Flux_alpha+1,a ;6mov a,r5 ;8 R5 = [ I_alpha * STATOR_L]_Hdec r0addc a,@r0 ;10 *R0 = [ integral(V_alpha + I_alpha * STATOR_R)dt ]_H mov gi_Flux_alpha,a ;12mov r0,#gi2_Integrator_beta ;14mov MDU_MD4,#FLUX_PT1_Z1_L ;18 MDU readymov MDU_MD5,#FLUX_PT1_Z1_H ;mov r7,CD_CORDYH ; R7 = [V_beta + I_beta * STATOR_R]_Hmov r6,CD_CORDYL ; R6 = [V_beta + I_beta * STATOR_R]_Lmov r5,MDU_MR3 ; R5 = [I_beta * STATOR_L]_Hmov r4,MDU_MR2 ; R4 = [I_beta * STATOR_L]_L;**************************************;* integration by PT1 integral(r6,r7)dt;*clr F0 ; F0 = 0 for PT1 scalermov r3,#FLUX_PT1_Z2_Hmov r2,#FLUX_PT1_Z2_Llcall PT1_MDU ; PT1-return value L = *R1 , H = *R0;**************************************;* CD_CORDY = [R5 + *R0, R4 + *R1];*mov a,r4 ; R4 = [ I_beta * STATOR_L ]_Ladd a,@r0 ; *R1 = [ integral(V_beta + I_beta * STATOR_R)dt ]_L mov CD_CORDYL,a ;mov a,r5 ; R5 = [ I_beta * STATOR_L]_Hdec r0addc a,@r0 ; *R0 = [ integral(V_beta + I_beta * STATOR_R)dt ]_H mov CD_CORDYH,a;**************************************;* calculation angle = atan( Flux_beta / Flux_alpha );*clr amov CD_STATC,a ; clear keep bits - interrupt disabledmov CD_CON,#0x02 ; Circular vectoring mode - auto start MPS = 1 mov CD_CORDZH,a ; Z = 0mov CD_CORDZL,a ;mov CD_CORDXH,gi_Flux_alpha ; X = Flux_alphamov CD_CORDXL,gi_Flux_alpha+1 ; CORDIC starts automaticallymov r7,guc_SpeedControlCounter ;4cjne r7,#0,_fb_CD0 ;6mov gi_Angle_mem,gi_angle ;10 store previous angle in angle_memmov gi_Angle_mem+1,gi_angle+1 ;14;**************************************;* inverse vector rotation;**************************************_fb_CD0:mov CD_CORDYL,gi_I_alpha+1 ;18 Y = Alphamov CD_CORDYH,gi_I_alpha ;22mov CD_CORDXH,gi_I_beta ;26 X = Beta;**************************************;* flux estimation continued;**************************************_fb_CD1:jb CD_BSY,_fb_CD1 ; wait for CORDIC result of angle calculationjnb gb_closed_loop,_fb_CD2a ; if( gb_closed_loop == 1 )mov gi_angle+1,CD_CORDZL ; {mov gi_angle,CD_CORDZH ; angle = atan( Flux_alpha / Flux_beta )sjmp _fb_CD2 ; }_fb_CD2a: ; during startup turn motor in open loop mov gi_Flux_angle+1,CD_CORDZL ; elsemov gi_Flux_angle,CD_CORDZH ; angle1 = atan( Flux_alpha / Flux_beta )mov MDU_MD0,gi_delta_angle+1 ; gi_delta_angle * gi_V_q_vf_slewmov MDU_MD1,gi_delta_anglemov MDU_MD4,gi_V_q_vf_slew+1mov MDU_MD5,gi_V_q_vf_slewmov MDU_MDUCON,#0x14; ; start Multiplikationmov R0,#gi_delta_angle+1 ; gi_angle += gi_delta_anglemov a,gi_angle+1 ; add delta angle at start-upadd a,@R0mov gi_angle+1,amov a,gi_angledec R0addc a,@R0mov gi_angle,amov r1,#gt_Iq_control+2clr amov @r1,adec r1mov R0,#gi_V_q_vf_offset+1 ; gi_V_q = gi_V_q_vf_offset + gi_delta_angle * gi_V_q_vf_slew * 256 mov a,MDU_MD1add a,@R0mov gi_V_q+1,amov @r1,amov a,MDU_MD2dec R0dec R1addc a,@R0mov gi_V_q,amov @r1,a;**************************************;* inverse vector rotation continued;**************************************;*;* rotate I_alpha and I_beta by angle;*;* input gi_I_alpha;* gi_I_beta;* gi_angle;*;* output gi_Id;* gi_Iq;*_fb_CD2:mov CD_STATC,a ; clear keep bits - interrupt disabledmov CD_CON,#0x4A ; Circular rotation mode - auto start MPS = 2 mov CD_CORDZH,gi_angle ; Z = gi_anglemov CD_CORDZL,gi_angle+1mov CD_CORDXL,gi_I_beta+1 ; CORDIC starts autmatically_vdm_CD0:jb CD_BSY,_vdm_CD0 ; wait for CORDIC result of inverse vector rotationmov gi_I_q+1,CD_CORDXL ; _gi_Iqmov gi_I_q,CD_CORDXH ;mov gi_I_d+1,CD_CORDYL ; _gi_Idmov gi_I_d,CD_CORDYH ;// uncomment this if scaling of gi_I_q to fullscale (8000 ... 7fff) is required// mov MDU_MD0,gi_I_q+1 ; MD0 = gi_I_q_L// mov MDU_MD1,gi_I_q ; MD1 = gi_I_q_H// mov MDU_MD4,#0xB8 ; MD4 = 16/K / 256 *2 _L // mov MDU_MD5,#0x09 ; MD5 = 16/K / 256 *2 _H // mov MDU_MDUCON,#13H; ; Start multiplication// nop// nop// nop// nop// nop// mov gi_I_q+1,MDU_MR1// mov gi_I_q,MDU_MR2 ; gi_I_q = gi_I_q * 16/K;**************************************;* Speed Calculation;**************************************;*;* calculation of speed = (angle_mem - angle) / T12PERIODE_HALF;*;* speed calculation is executed every;* (SPEED_CONTROL_RATE/CURRENT_CONTROL_RATE) of control loop;*;* input gi_angle;* gi_Angle_mem;* gi_Speed;*;* output *R0 = gi_Speed_H;* *R1 = gi_Speed_L;inc guc_SpeedControlCountermov a,guc_SpeedControlCounter ; if( ++_guc_SpeedControlCounter != (SPEED_CONTROL_RATE/CURRENT_CONTROL_RATE) ) jmp _node3_int1 cjne a,#(SPEED_CONTROL_RATE/CURRENT_CONTROL_RATE),_node3_int1mov guc_SpeedControlCounter,#0x00 ; else _guc_SpeedControlCounter = 0;clr c ; C = 0mov a,gi_angle+1 ; A = angle_Lsubb a,gi_Angle_mem+1 ; A = angle_L - angle_mem_Lmov r6,a ; R6 = (angle - angle_mem)_L = angle_L - angle_mem_Lmov a,gi_angle ; A = angle_Hsubb a,gi_Angle_mem ; A = angle_H - angle_mem_Hmov r7,a ; R7 = angle_H - angle_mem_Hmov r0,#gi_Speed+2mov a,#SPEED_PT1_Z;**************************************;* PT1 with 24bit memory;**************************************;*;* calculation of Y_(k) = Y_(k-1) + 2^(-Z) * { X_(k) - Y_(k-1) };*;* input Y [HLh] *R0 = Y_h;* X R6 = L, R7 = H;* Z a [1..15];*;* output *R0 = Y_H;*;* remark no limitation of subtract and add;* => X must be smaller than 0x3fff;*setb acc.5 ; select right shiftmov MDU_MD4,a ; MD4 = number of bits to shiftclr cclr asubb a,@r0 ; A = e_h = 0 - Y_hmov MDU_MD1,a ; MD1 = e_hdec r0mov a,r6 ; A = X_Lsubb a,@r0 ; A = e_L = X_L - Y_Lmov MDU_MD2,a ; MD2 = e_Ldec r0mov a,r7 ; A = X_Hsubb a,@r0 ; A = e_H = X_H - Y_Hmov MDU_MD3,a ; MD3 = e_Hmov MDU_MDUCON,#0x17; ; Start right-shift (X - Y)_HLh >> Z //wait for XC88x - no ws no double clknopnopnopnop//...inc r0 ;4inc r0 ;8nop ;12mov a,@r0 ;16 a = Y_hadd a,MDU_MR1 ; A = Y_h(k-1) + MR1mov @r0,a ; Y_h(k) = Y_h(k-1) + MR1dec r0mov a,@r0 ; a = Y_Laddc a,MDU_MR2 ; A = Y_L(k-1) + MR2mov @r0,a ; Y_L(k) = Y_L(k-1) + MR2mov r4,adec r0mov a,@r0addc a,MDU_MR3 ; A = Y_H(k-1) + MR3 + Cmov @r0,a ; Y_H(k) = Y_H(k-1) + MR3 + Cmov r5,a;* end of PT1 with 24bit memory;**************************************;**************************************;* Speed and Current Control;**************************************jb gb_closed_loop,_node3_int0 ; if( _gb_closed_loop == 1 ) jmp _node3_int0 ajmp _node3_int2 ; else jmp _node3_int2_node3_int0: ;;**************************************;* Speed Control;**************************************;*;* Load parameters for speed control;* call PI_controller;* use output as Iq_reference;*mov r7,gi_Speed_referencemov r6,gi_Speed_reference+1mov r0,#gt_Speed_control+1+5#ifdef SPEED_PI_KP_GAIN_128setb F0 ;Kp gain = 128#elseclr F0 ;Kp gain = 8#endiflcall PI_controller;* Limitation of IQ to positive (do not active brake) ; mov a,r6; jnb acc.7,_limit_end; mov r6, #0h; mov r7, #0h_limit_end:#ifdef LIMIT_IQ;* Limitation of speed control outputmov a,r6jnb acc.7,_limit_positivemov a, #LIMIT_IQ_Ladd a,r7mov a, #LIMIT_IQ_Haddc a,r6jc _limit_endmov r6, #~LIMIT_IQ_Hmov r7, #~LIMIT_IQ_Lsjmp _limit_end_limit_positive:mov a, #LIMIT_IQ_Lclr csubb a,r7mov a, #LIMIT_IQ_Hsubb a,r6jnc _limit_endmov r6, #LIMIT_IQ_Hmov r7, #LIMIT_IQ_L_limit_end:#endifmov gi_Iq_reference,r6mov gi_Iq_reference+1,r7_node3_int1:;************************************** ;* Current Control Id;************************************** ;*;* Load parameters for Id control;* call PI_controller and;* store result in Vd;*;* input;* gi_Id_reference;* gi_Id;*;* output;* gi_V_d;*mov r7,gi_Id_referencemov r6,gi_Id_reference+1mov r5,gi_I_dmov r4,gi_I_d+1mov r0,#gt_Id_control+1+5clr F0 ;Kp gain = 8lcall PI_controllermov gi_V_d,r6mov gi_V_d+1,r7;************************************** ;* Current Control Iq;************************************** ;*;* Load parameters for Iq control;* call PI_controller;* store result in Vq;*;* input;* gi_Iq_reference;* gi_Iq;*;* output;* gi_V_q;*mov r7,gi_Iq_referencemov r6,gi_Iq_reference+1mov r5,gi_I_qmov r4,gi_I_q+1mov r0,#gt_Iq_control+1+5clr F0 ;Kp gain = 8lcall PI_controllermov gi_V_q,r6mov gi_V_q+1,r7_node3_int2:;**************************************;* vector rotation;**************************************;*;* rotate gi_V_q and gi_V_d by angle;*;* input gi_V_q;* gi_V_d;* gi_angle;*;* output gi_V_alpha;* gi_V_beta;*clr amov CD_STATC,a ; clear keep bits - interrupt disabledmov CD_CON,#0x8A ; Circular rotation mode - auto start MPS = 4 mov CD_CORDZH,gi_angle ; Z = gi_anglemov CD_CORDZL,gi_angle+1mov CD_CORDYL,gi_V_q+1 ; Y = Q_kompmov CD_CORDYH,gi_V_q ;mov CD_CORDXH,gi_V_d ; X = D_kompmov CD_CORDXL,gi_V_d+1 ; CORDIC starts autmaticallymov CD_CORDZH,a ; Z = 0mov CD_CORDZL,a_MD_0:jb CD_BSY,_MD_0 ; wait for CORDIC with vector rotationmov r7,CD_CORDXL ; R7 <- V_alpha_Lmov r6,CD_CORDXH ; R6 <- V_alpha_Hmov r5,CD_CORDYL ; R5 <- V_beta_Lmov r4,CD_CORDYH ; R4 <- V_beta;**************************************;* catresian to polar transform;**************************************;*;* calculate space vector amplitude (norm);*;* input gi_V_alpha;* gi_V_beta;* R7 <- V_alpha_L;* R6 <- V_alpha_H;* R5 <- V_beta_L;* R4 <- V_beta;*;* output gi_V_alpha_L;* gi_V_alpha_H;* gi_V_beta_L;* gi_V_beta;* R7 = gi_Amplitude_L;* R6 = gi_Amplitude_H;* CORDZ = Anglemov CD_STATC,a ; clear keep bits - interrupt disabledmov CD_CON,#0x82 ; Circular vectoring mode - auto start MPS = 4mov CD_CORDYL,r5 ; Y = gi_V_betamov CD_CORDYH,r4 ;mov CD_CORDXH,r6 ; X = gi_V_alphamov CD_CORDXL,r7 ; CORDIC starts autmaticallymov MDU_MD0,#0x06 ; MD0 = 6mov MDU_MD1,a ; MD1 = 0mov gi_V_alpha+1,r7 ; gi_V_alpha_L = R7mov gi_V_alpha,r6 ; gi_V_alpha_H = R6mov gi_V_beta+1,r5 ; gi_V_beta_L = R5mov gi_V_beta,r4 ; gi_V_beta = R4_MD_02:jb CD_BSY,_MD_02 ; wait for CORDIC result of amplitude and angle calculationmov r7,CD_CORDXLmov r6,CD_CORDXH;* limitation of Amplitudeclr c ; if ( MAX_AMPLITUDE - gi_Amplitude ) < 0mov a,#MAX_AMPLITUDE_L ;subb a,r7 ;mov a,#MAX_AMPLITUDE_H ;subb a,r6 ;jnc _MD_002 ;mov r7,#MAX_AMPLITUDE_L ; gi_Amplitude = MAX_AMPLITUDEmov r6,#MAX_AMPLITUDE_H_MD_002: ; elsemov gi_Amplitude+1,r7 ; R7 = gi_Amplitude_L mov gi_Amplitude,r6 ; R6 = gi_Amplitude_H;**************************************;* sv modulation;**************************************;*;* space vector modulation;*;* input guc_sector;* R7 = gi_Amplitude_L;* R6 = gi_Amplitude_H;* CORDZ = Angle;*;* output;* R0 = V_TA_H;* R1 = V_TA_L;* R2 = V_TB_H;* R3 = V_TB_L;* R4 = V_TC_H;* a = V_TC_L;* R5 = T1/2_L;* B = T1/2_H;* R6 = T2_H/2;* R7 = T2_L/2;*mov MDU_MD4,CD_CORDZL ; MD4 = Z = Anglemov MDU_MD5,CD_CORDZH ; MD5 = Z = Anglemov MDU_MDUCON,#0x10; ; start Multiplikation (gi_angle * 6) -> Sektor,gammamov MDU_MD4,r7 ;4 MD4 = amp_Lmov MDU_MD5,r6 ;8 MD5 = amp_H_MD_01:clr a ;10mov CD_STATC,a ;12 clear keep bits - interrupt disabledmov CD_CORDZL,a ;14 Z = 0mov dptr,#Sinus60_tab ; MDU ready / get pointer at Sinustablemov r2,MDU_MR1 ; R2 = gamma 8bitmov guc_sector,MDU_MR2 ; R3 = guc_sector 0 (5)mov a,r2 ; A = gamma 8bitadd a,ACC ; A = gamma 9bit 256 16Bits valuesmov r0,a ; R0 = gamma 9bit 256 16Bits valuesjnc _MD_1inc dph_MD_1:;*;* Calculate T2;*movc a,@a+dptr ; A = sinus_Hmov MDU_MD1,a ; MD1 = sinus_Hmov a,r0 ; A = gammainc amovc a,@a+dptr ; A = sinus_Lmov MDU_MD0,a ; MD0 = sinus_Lmov MDU_MDUCON,#0x14; ; start Multiplikation amp * sinus(gamma);*;* Calculate T1;*mov dptr,#Sinus60_tabmov a,r2cpl aadd a,ACCmov r2,ajnc _MD_2inc dph_MD_2:movc a,@a+dptr ; A = sinus_H , MDU fertigmov MDU_MD1,a ; MD1 = sinus_Hmov r5,MDU_MR2 ; R5 = T2_Lmov r4,MDU_MR3 ; R4 = T2_Hmov a,r2 ; A = gamma 8bitinc amovc a,@a+dptr ; A = sinus_Lmov MDU_MD0,a ; MD0 = sinus_Lmov MDU_MDUCON,#0x14; ; start Multiplikation amp * sinus(60 - gamma)mov CD_CON,#0x48 ;4 Linear rotation mode - auto start MPS = 2 mov CD_CORDZH,#0x08 ;8 Z = 1mov CD_CORDYH,r4 ;12 Y = T2jnb gb_closed_loop,_MD_3 ; ignore TMIN during start-upcjne r4,#0,_MD_3 ;16 , MDU readymov a,#T_MINsubb a,r5jc _MD_3mov r5,#T_MIN_MD_3:mov r2,MDU_MR3 ; R2 = T1_Hmov r1,MDU_MR2 ; R1 = T1_Ljnb gb_closed_loop,_MD_4 ; ignore TMIN during start-upcjne r2,#0,_MD_4mov a,#T_MINsubb a,r1jc _MD_4mov r1,#T_MIN_MD_4:mov CD_CORDYL,r5 ; Y = T2mov CD_CORDXH,r2 ; X = T1mov CD_CORDXL,r1 ; CORDIC starts autmaticallyclr c ;2 Berechnung T2 /2mov a,r4 ;4 A = T2_Hrrc a ;6 A = T2_H/2mov r6,a ;8 R6 = T2_H/2mov a,r5 ;10 A = T2_Lrrc a ;12 A = T2_L/2mov r7,a ;14 R7 = T2_L/2mov a,guc_sector ;16jb ACC.0,_MD_5 ;20mov a,r1mov r5,a ; R5 = T1_L/T2_Lmov a,r2mov r4,a ; R4 = T1_L/T2_L_MD_5:mov r2,#T12PERIODE_HALF_H ;22mov r1,#T12PERIODE_HALF_H ;24mov a,#T12PERIODE_HALF_L ;26mov r0,a ;28inc guc_sector ;30 preparation for djnz instruction mov guc_sector_s,guc_sector ;34_MD_6:jb CD_BSY,_MD_6 ;CORDIC readyadd a,CD_CORDYLxch a,r1 ; R1 = TT3_L A = 1addc a,CD_CORDYHxch a,r0 ; R0 = TT3_H A = 144;*;* Calculate TT1;*clr csubb a,CD_CORDYLmov r3,a ; R3 = TT1_Lxch a,r2 ; R2 = TT1_L A = 1subb a,CD_CORDYH ; A = TT1_Hxch a,r2 ; R2 = TT1_H / A = T1_Ladd a,r5 ; A = TT1_L + T1,2_Lxch a,r4 ; R4 = TT1_L + T1,2_Laddc a,r2 ; A = TT1_H + T1,2_Hxch a,r4 ; R4 = TT1_H + T1,2_Hmov r5,CD_CORDXL ; R5 = T1/2_Lmov b,CD_CORDXH ; B = T1/2_H;**************************************;* update compare values;**************************************;*;* Calculation and setup of compare;* values of PWM Unit CCU6;*;* input guc_sector;* R0 = V_TA_H;* R1 = V_TA_L;* R2 = V_TB_H;* R3 = V_TB_L;* R4 = V_TC_H;* a = V_TC_L;* R5 = T1/2_L;* B = T1/2_H;* R6 = T2_H/2;* R7 = T2_L/2;*;* output;* setup of CCU6 registers;*;*;* clr RMAP for normal operation;*anl SYSCON0,#0xFE ; switch to standard SFRsmov CCU6_PAGE,#0x00CV_SectorA:djnz guc_sector,CV_SectorBmov CCU6_CC60RH,r2mov CCU6_CC60RL,r3 ; CC60CR = V_TBmov CCU6_CC61RL,amov CCU6_CC61RH,r4 ; CC61CR = V_TCmov CCU6_CC62RH,r0mov CCU6_CC62RL,r1 ; CC60CR = V_TAmov CCU6_PAGE,#0x00mov a,#AD_TRIGGER_CM_L ; A = AD_TRIGGER_CM_Ladd a,r7 ; A = AD_TRIGGER_CM_L + T2/2_L mov CCU6_CC63RL,amov a,#AD_TRIGGER_CM_H ; A = AD_TRIGGER_CM_Haddc a,r6 ; A = AD_TRIGGER_CM_H + T2/2_Hmov CCU6_PAGE,#0x01mov a,#AD_TRIGGER_PM_L ; A = AD_TRIGGER_PM_Ladd a,r5 ; A = AD_TRIGGER_PM_L + T1/2_L mov CCU6_T13PRL,amov a,#AD_TRIGGER_PM_H ; A = AD_TRIGGER_PM_Haddc a,b ; A = AD_TRIGGER_PM_H + T1/2_H mov CCU6_T13PRH,aljmp compare_values_endCV_SectorB:djnz guc_sector,CV_SectorCmov CCU6_CC60RL,amov CCU6_CC60RH,r4 ; CC60CR = V_TCmov CCU6_CC61RH,r2mov CCU6_CC61RL,r3 ; CC61CR = V_TBmov CCU6_CC62RH,r0mov CCU6_CC62RL,r1 ; CC60CR = V_TAmov CCU6_PAGE,#0x00mov a,#AD_TRIGGER_CM_L ; A = AD_TRIGGER_CM_Ladd a,r5 ; A = AD_TRIGGER_CM_L + T1/2_L mov CCU6_CC63RL,amov a,#AD_TRIGGER_CM_H ; A = AD_TRIGGER_CM_Haddc a,b ; A = AD_TRIGGER_CM_H + T1/2_H mov CCU6_CC63RH,amov CCU6_PAGE,#0x01mov a,#AD_TRIGGER_PM_L ; A = AD_TRIGGER_PM_Ladd a,r7 ; A = AD_TRIGGER_PM_L + T2/2_Lmov a,#AD_TRIGGER_PM_H ; A = AD_TRIGGER_PM_Haddc a,r6 ; A = AD_TRIGGER_PM_H + T2/2_H mov CCU6_T13PRH,aljmp compare_values_endCV_SectorC:djnz guc_sector,CV_SectorDmov CCU6_CC60RH,r0mov CCU6_CC60RL,r1 ; CC60CR = V_TAmov CCU6_CC61RH,r2mov CCU6_CC61RL,r3 ; CC60CR = V_TBmov CCU6_CC62RL,amov CCU6_CC62RH,r4 ; CC61CR = V_TCmov CCU6_PAGE,#0x00mov a,#AD_TRIGGER_CM_L ; A = AD_TRIGGER_CM_Ladd a,r7 ; A = AD_TRIGGER_CM_L + T2/2_L mov CCU6_CC63RL,amov a,#AD_TRIGGER_CM_H ; A = AD_TRIGGER_CM_Haddc a,r6 ; A = AD_TRIGGER_CM_H + T2/2_H mov CCU6_CC63RH,amov CCU6_PAGE,#0x01mov a,#AD_TRIGGER_PM_L ; A = AD_TRIGGER_PM_Ladd a,r5 ; A = AD_TRIGGER_PM_L + T1/2_L mov CCU6_T13PRL,amov a,#AD_TRIGGER_PM_H ; A = AD_TRIGGER_PM_Haddc a,b ; A = AD_TRIGGER_PM_H + T1/2_H mov CCU6_T13PRH,a。
