threephaseengine12.asm

microchip公司的PICPIC18F系列芯片固件

;
; Square and accumulate phase A voltage
;
	btfss	PhA_V_D1_15_8,7,A		; Check for negative number
	bra	LPI_PhA_V_Pos			; Is positive, skip two's complement
	comf	PhA_V_D1_15_8,F,A		; Is negative, two's complement the 16-bit value
	negf	PhA_V_D1_7_0,A
	btfsc	STATUS,C,A
	incf	PhA_V_D1_15_8,F,A
LPI_PhA_V_Pos
	movf	PhA_V_D1_7_0,W,A		; Arg1L * Arg1L -> PRODH:PRODL -> add to bits 15:0 of accum
	mulwf	PhA_V_D1_7_0,A
	movf	PRODL,W,A
	addwf	PhA_V_WAcc_7_0,F,A
	movf	PRODH,W,A
	addwfc	PhA_V_WAcc_15_8,F,A
	clrf	WREG,A
	addwfc	PhA_V_WAcc_23_16,F,A
	addwfc	PhA_V_WAcc_31_24,F,A
	addwfc	PhA_V_WAcc_39_32,F,A
	movf	PhA_V_D1_15_8,W,A		; Arg1H * Arg1H -> PRODH:PRODL -> add to bits 31:16 of accum
	mulwf	PhA_V_D1_15_8,A
	movf	PRODL,W,A
	addwf	PhA_V_WAcc_23_16,F,A
	movf	PRODH,W,A
	addwfc	PhA_V_WAcc_31_24,F,A
	clrf	WREG,A
	addwfc	PhA_V_WAcc_39_32,F,A
	rlncf	PhA_V_D1_15_8,W,A		; Cross products: Arg1L * (2*Arg1H) -> PRODH:PRODL ->
	mulwf	PhA_V_D1_7_0,A			;  add to bits 39:8 of accum
	movf	PRODL,W,A			; (Note: the rotate to produce 2*Arg1H works because
	addwf	PhA_V_WAcc_15_8,F,A		;  the initial value is between 0x00 and 0x7F, multiplying
	movf	PRODH,W,A			;  by two means the cross product only has to be added
	addwfc	PhA_V_WAcc_23_16,F,A		;  once rather than twice.)
	clrf	WREG,A
	addwfc	PhA_V_WAcc_31_24,F,A
	addwfc	PhA_V_WAcc_39_32,F,A


;
; Phase correction for phase B
;
	movf	PhB_V_D1_7_0,W,A		; Subtract most recent voltage from previous voltage
	subwf	PhB_V_7_0,W,A
	movwf	PhB_V_P_7_0,A
	movf	PhB_V_D1_15_8,W,A
	subwfb	PhB_V_15_8,W,A
	movwf	LPI_temp1,A

	movf	PhB_DelayX2,W,A			; Get phase correction amplitude
	mulwf	PhB_V_P_7_0,A			; Arg1L * Arg2L -> PRODH -> PhB_V_P_7_0
	movff	PRODH,PhB_V_P_7_0		;  (don't need PRODL and done with old PhB_V_P_7_0)
	mulwf	LPI_temp1,A			; Multiply for cross product (Arg1L * Arg1H) ->
	movf	PRODL,W,A			; Add result
	addwf	PhB_V_P_7_0,F,A
	movff	PRODH,PhB_V_P_15_8
	btfsc	STATUS,C,A
	incf	PhB_V_P_15_8,F,A
	btfss	LPI_temp1,7,A			; Difference negative?
	bra	LPI_PhB_Diff_Done		; No, done
	movf	PhB_DelayX2,A
	subwf	PhB_V_P_15_8,F,A
LPI_PhB_Diff_Done

	movf	PhB_V_D1_7_0,W,A		; Add portion of difference to previous voltage
	addwf	PhB_V_P_7_0,F,A
	movf	PhB_V_D1_15_8,W,A
	addwfc	PhB_V_P_15_8,F,A

	btfsc	Flags1,PhB_Delay_SignBit,A	; Sign of phase correction determines current value to use
	bra	LPI_PhB_PC_Neg
	movff	PhB_I_D1_7_0,PhB_I_P_7_0	; If positive, use delayed value
	movff	PhB_I_D1_15_8,PhB_I_P_15_8
	bra	LPI_PhB_PC_Done
LPI_PhB_PC_Neg
	movff	PhB_I_7_0,PhB_I_P_7_0		; If negative, use most recent value
	movff	PhB_I_15_8,PhB_I_P_15_8
LPI_PhB_PC_Done

;
; Phase B power = V*I and accumulate
;
	movf	PhB_V_P_7_0,W,A			; Arg1L * Arg2L -> PRODH:PRODL -> LPI_Res1:LPI_Res0
	mulwf	PhB_I_P_7_0,A
	movff	PRODH,LPI_Res1
	movff	PRODL,LPI_Res0
	movf	PhB_V_P_15_8,W,A		; Arg1H * Arg2H -> PRODH:PRODL -> LPI_Res3:LPI_Res2
	mulwf	PhB_I_P_15_8,A
	movff	PRODH,LPI_Res3
	movff	PRODL,LPI_Res2
	movf	PhB_V_P_7_0,W,A			; Cross products: Arg1L * Arg2H -> PRODH:PRODL ->
	mulwf	PhB_I_P_15_8,A			;  add to LPI_Res3:LPI_Res1
	movf	PRODL,W,A
	addwf	LPI_Res1,F,A
	movf	PRODH,W,A
	addwfc	LPI_Res2,F,A
	clrf	WREG,A
	addwfc	LPI_Res3,F,A
	movf	PhB_V_P_15_8,W,A		; Cross products: Arg1H * Arg2L -> PRODH:PRODL ->
	mulwf	PhB_I_P_7_0,A			;  add to LPI_Res3:LPI_Res1
	movf	PRODL,W,A
	addwf	LPI_Res1,F,A
	movf	PRODH,W,A
	addwfc	LPI_Res2,F,A
	clrf	WREG,A
	addwfc	LPI_Res3,F,A
	btfss	PhB_I_P_15_8,7,A		; Current negative?
	bra	LPI_PhB_P_ChkNegV		; No, check voltage
	movf	PhB_V_P_7_0,W,A
	subwf	LPI_Res2,F,A
	movf	PhB_V_P_15_8,W,A
	subwfb	LPI_Res3,F,A
LPI_PhB_P_ChkNegV
	btfss	PhB_V_P_15_8,7,A		; Voltage negative?
	bra	LPI_PhB_P_SqrDone		; No, done
	movf	PhB_I_P_7_0,W,A
	subwf	LPI_Res2,F,A
	movf	PhB_I_P_15_8,W,A
	subwfb	LPI_Res3,F,A
LPI_PhB_P_SqrDone

	movf	LPI_Res0,W,A			; Accumulate phase B power
	addwf	PhB_W_WAcc_7_0,F,A
	movf	LPI_Res1,W,A
	addwfc	PhB_W_WAcc_15_8,F,A
	movf	LPI_Res2,W,A
	addwfc	PhB_W_WAcc_23_16,F,A
	movf	LPI_Res3,W,A
	addwfc	PhB_W_WAcc_31_24,F,A
	clrf	WREG,A				; Add carry & sign extend
	btfsc	LPI_Res3,7,A			;  if needed
	comf	WREG,F,A
	addwfc	PhB_W_WAcc_39_32,F,A

;
; Square and accumulate phase B current
;
	btfss	PhB_I_D1_15_8,7,A		; Check for negative number
	bra	LPI_PhB_I_Pos			; Is positive, skip two's complement
	comf	PhB_I_D1_15_8,F,A		; Is negative, two's complement the 16-bit value
	negf	PhB_I_D1_7_0,A
	btfsc	STATUS,C,A
	incf	PhB_I_D1_15_8,F,A
LPI_PhB_I_Pos
	movf	PhB_I_D1_7_0,W,A		; Arg1L * Arg1L -> PRODH:PRODL -> add to bits 15:0 of accum
	mulwf	PhB_I_D1_7_0,A
	movf	PRODL,W,A
	addwf	PhB_I_WAcc_7_0,F,A
	movf	PRODH,W,A
	addwfc	PhB_I_WAcc_15_8,F,A
	clrf	WREG,A
	addwfc	PhB_I_WAcc_23_16,F,A
	addwfc	PhB_I_WAcc_31_24,F,A
	addwfc	PhB_I_WAcc_39_32,F,A
	movf	PhB_I_D1_15_8,W,A		; Arg1H * Arg1H -> PRODH:PRODL -> add to bits 31:16 of accum
	mulwf	PhB_I_D1_15_8,A
	movf	PRODL,W,A
	addwf	PhB_I_WAcc_23_16,F,A
	movf	PRODH,W,A
	addwfc	PhB_I_WAcc_31_24,F,A
	clrf	WREG,A
	addwfc	PhB_I_WAcc_39_32,F,A
	rlncf	PhB_I_D1_15_8,W,A		; Cross products: Arg1L * (2*Arg1H) -> PRODH:PRODL ->
	mulwf	PhB_I_D1_7_0,A			;  add to bits 39:8 of accum
	movf	PRODL,W,A			; (Note: the rotate to produce 2*Arg1H works because
	addwf	PhB_I_WAcc_15_8,F,A		;  the initial value is between 0x00 and 0x7F, multiplying
	movf	PRODH,W,A			;  by two means the cross product only has to be added
	addwfc	PhB_I_WAcc_23_16,F,A		;  once rather than twice.)
	clrf	WREG,A
	addwfc	PhB_I_WAcc_31_24,F,A
	addwfc	PhB_I_WAcc_39_32,F,A

;
; Square and accumulate phase B voltage
;
	btfss	PhB_V_D1_15_8,7,A		; Check for negative number
	bra	LPI_PhB_V_Pos			; Is positive, skip two's complement
	comf	PhB_V_D1_15_8,F,A		; Is negative, two's complement the 16-bit value
	negf	PhB_V_D1_7_0,A
	btfsc	STATUS,C,A
	incf	PhB_V_D1_15_8,F,A
LPI_PhB_V_Pos
	movf	PhB_V_D1_7_0,W,A		; Arg1L * Arg1L -> PRODH:PRODL -> add to bits 15:0 of accum
	mulwf	PhB_V_D1_7_0,A
	movf	PRODL,W,A
	addwf	PhB_V_WAcc_7_0,F,A
	movf	PRODH,W,A
	addwfc	PhB_V_WAcc_15_8,F,A
	clrf	WREG,A
	addwfc	PhB_V_WAcc_23_16,F,A
	addwfc	PhB_V_WAcc_31_24,F,A
	addwfc	PhB_V_WAcc_39_32,F,A
	movf	PhB_V_D1_15_8,W,A		; Arg1H * Arg1H -> PRODH:PRODL -> add to bits 31:16 of accum
	mulwf	PhB_V_D1_15_8,A
	movf	PRODL,W,A
	addwf	PhB_V_WAcc_23_16,F,A
	movf	PRODH,W,A
	addwfc	PhB_V_WAcc_31_24,F,A
	clrf	WREG,A
	addwfc	PhB_V_WAcc_39_32,F,A
	rlncf	PhB_V_D1_15_8,W,A		; Cross products: Arg1L * (2*Arg1H) -> PRODH:PRODL ->
	mulwf	PhB_V_D1_7_0,A			;  add to bits 39:8 of accum
	movf	PRODL,W,A			; (Note: the rotate to produce 2*Arg1H works because
	addwf	PhB_V_WAcc_15_8,F,A		;  the initial value is between 0x00 and 0x7F, multiplying
	movf	PRODH,W,A			;  by two means the cross product only has to be added
	addwfc	PhB_V_WAcc_23_16,F,A		;  once rather than twice.)
	clrf	WREG,A
	addwfc	PhB_V_WAcc_31_24,F,A
	addwfc	PhB_V_WAcc_39_32,F,A


;
; Phase correction for phase C
;
	movf	PhC_V_D1_7_0,W,A		; Subtract most recent voltage from previous voltage
	subwf	PhC_V_7_0,W,A
	movwf	PhC_V_P_7_0,A
	movf	PhC_V_D1_15_8,W,A
	subwfb	PhC_V_15_8,W,A
	movwf	LPI_temp1,A

	movf	PhC_DelayX2,W,A			; Get phase correction amplitude
	mulwf	PhC_V_P_7_0,A			; Arg1L * Arg2L -> PRODH -> PhC_V_P_7_0
	movff	PRODH,PhC_V_P_7_0		;  (don't need PRODL and done with old PhC_V_P_7_0)
	mulwf	LPI_temp1,A			; Multiply for cross product (Arg1L * Arg1H) ->
	movf	PRODL,W,A			; Add result
	addwf	PhC_V_P_7_0,F,A
	movff	PRODH,PhC_V_P_15_8
	btfsc	STATUS,C,A
	incf	PhC_V_P_15_8,F,A
	btfss	LPI_temp1,7,A			; Difference negative?
	bra	LPI_PhC_Diff_Done		; No, done
	movf	PhC_DelayX2,A
	subwf	PhC_V_P_15_8,F,A
LPI_PhC_Diff_Done

	movf	PhC_V_D1_7_0,W,A		; Add portion of difference to previous voltage
	addwf	PhC_V_P_7_0,F,A
	movf	PhC_V_D1_15_8,W,A
	addwfc	PhC_V_P_15_8,F,A

	btfsc	Flags1,PhC_Delay_SignBit,A	; Sign of phase correction determines current value to use
	bra	LPI_PhC_PC_Neg
	movff	PhC_I_D1_7_0,PhC_I_P_7_0	; If positive, use delayed value
	movff	PhC_I_D1_15_8,PhC_I_P_15_8
	bra	LPI_PhC_PC_Done
LPI_PhC_PC_Neg
	movff	PhC_I_7_0,PhC_I_P_7_0		; If negative, use most recent value
	movff	PhC_I_15_8,PhC_I_P_15_8
LPI_PhC_PC_Done

;
; Phase C power = V*I and accumulate
;
	movf	PhC_V_P_7_0,W,A			; Arg1L * Arg2L -> PRODH:PRODL -> LPI_Res1:LPI_Res0
	mulwf	PhC_I_P_7_0,A
	movff	PRODH,LPI_Res1
	movff	PRODL,LPI_Res0
	movf	PhC_V_P_15_8,W,A		; Arg1H * Arg2H -> PRODH:PRODL -> LPI_Res3:LPI_Res2
	mulwf	PhC_I_P_15_8,A
	movff	PRODH,LPI_Res3
	movff	PRODL,LPI_Res2
	movf	PhC_V_P_7_0,W,A			; Cross products: Arg1L * Arg2H -> PRODH:PRODL ->
	mulwf	PhC_I_P_15_8,A			;  add to LPI_Res3:LPI_Res1
	movf	PRODL,W,A
	addwf	LPI_Res1,F,A
	movf	PRODH,W,A
	addwfc	LPI_Res2,F,A
	clrf	WREG,A
	addwfc	LPI_Res3,F,A
	movf	PhC_V_P_15_8,W,A		; Cross products: Arg1H * Arg2L -> PRODH:PRODL ->
	mulwf	PhC_I_P_7_0,A			;  add to LPI_Res3:LPI_Res1
	movf	PRODL,W,A
	addwf	LPI_Res1,F,A
	movf	PRODH,W,A
	addwfc	LPI_Res2,F,A
	clrf	WREG,A
	addwfc	LPI_Res3,F,A
	btfss	PhC_I_P_15_8,7,A		; Current negative?
	bra	LPI_PhC_P_ChkNegV		; No, check voltage
	movf	PhC_V_P_7_0,W,A
	subwf	LPI_Res2,F,A
	movf	PhC_V_P_15_8,W,A
	subwfb	LPI_Res3,F,A
LPI_PhC_P_ChkNegV
	btfss	PhC_V_P_15_8,7,A		; Voltage negative?
	bra	LPI_PhC_P_SqrDone		; No, done
	movf	PhC_I_P_7_0,W,A
	subwf	LPI_Res2,F,A
	movf	PhC_I_P_15_8,W,A
	subwfb	LPI_Res3,F,A
LPI_PhC_P_SqrDone

	movf	LPI_Res0,W,A			; Accumulate phase C power
	addwf	PhC_W_WAcc_7_0,F,A
	movf	LPI_Res1,W,A
	addwfc	PhC_W_WAcc_15_8,F,A
	movf	LPI_Res2,W,A
	addwfc	PhC_W_WAcc_23_16,F,A
	movf	LPI_Res3,W,A
	addwfc	PhC_W_WAcc_31_24,F,A
	clrf	WREG,A				; Add carry & sign extend
	btfsc	LPI_Res3,7,A			;  if needed
	comf	WREG,F,A
	addwfc	PhC_W_WAcc_39_32,F,A

;
; Square and accumulate phase C current
;
	btfss	PhC_I_D1_15_8,7,A		; Check for negative number
	bra	LPI_PhC_I_Pos			; Is positive, skip two's complement
	comf	PhC_I_D1_15_8,F,A		; Is negative, two's complement the 16-bit value
	negf	PhC_I_D1_7_0,A
	btfsc	STATUS,C,A
	incf	PhC_I_D1_15_8,F,A
LPI_PhC_I_Pos
	movf	PhC_I_D1_7_0,W,A		; Arg1L * Arg1L -> PRODH:PRODL -> add to bits 15:0 of accum
	mulwf	PhC_I_D1_7_0,A
	movf	PRODL,W,A
	addwf	PhC_I_WAcc_7_0,F,A
	movf	PRODH,W,A
	addwfc	PhC_I_WAcc_15_8,F,A
	clrf	WREG,A
	addwfc	PhC_I_WAcc_23_16,F,A
	addwfc	PhC_I_WAcc_31_24,F,A
	addwfc	PhC_I_WAcc_39_32,F,A
	movf	PhC_I_D1_15_8,W,A		; Arg1H * Arg1H -> PRODH:PRODL -> add to bits 31:16 of accum
	mulwf	PhC_I_D1_15_8,A
	movf	PRODL,W,A
	addwf	PhC_I_WAcc_23_16,F,A
	movf	PRODH,W,A
	addwfc	PhC_I_WAcc_31_24,F,A
	clrf	WREG,A
	addwfc	PhC_I_WAcc_39_32,F,A
	rlncf	PhC_I_D1_15_8,W,A		; Cross products: Arg1L * (2*Arg1H) -> PRODH:PRODL ->
	mulwf	PhC_I_D1_7_0,A			;  add to bits 39:8 of accum
	movf	PRODL,W,A			; (Note: the rotate to produce 2*Arg1H works because
	addwf	PhC_I_WAcc_15_8,F,A		;  the initial value is between 0x00 and 0x7F, multiplying
	movf	PRODH,W,A			;  by two means the cross product only has to be added
	addwfc	PhC_I_WAcc_23_16,F,A		;  once rather than twice.)
	clrf	WREG,A
	addwfc	PhC_I_WAcc_31_24,F,A
	addwfc	PhC_I_WAcc_39_32,F,A
;
; Square and accumulate phase C voltage
;
	btfss	PhC_V_D1_15_8,7,A		; Check for negative number
	bra	LPI_PhC_V_Pos			; Is positive, skip two's complement
	comf	PhC_V_D1_15_8,F,A		; Is negative, two's complement the 16-bit value
	negf	PhC_V_D1_7_0,A
	btfsc	STATUS,C,A
	incf	PhC_V_D1_15_8,F,A
LPI_PhC_V_Pos
	movf	PhC_V_D1_7_0,W,A		; Arg1L * Arg1L -> PRODH:PRODL -> add to bits 15:0 of accum
	mulwf	PhC_V_D1_7_0,A
	movf	PRODL,W,A
	addwf	PhC_V_WAcc_7_0,F,A
	movf	PRODH,