📄 rx5675_ptj.asm
字号:
nop
movlw (1<<CH_6)
nop
decfsz PWM_2
goto $-4
movwf PORTC
nop
movlw 0
nop
decfsz PWM_6
goto $-4
movwf PORTC
retlw 0
Out42X: ; ch4<ch2=ch6
movf PWM_4,w
subwf PWM_2
call Start246
nop
movlw (1<<CH_2)|(1<<CH_6)
nop
decfsz PWM_4
goto $-4
movwf PORTC
nop
movlw 0
nop
decfsz PWM_2
goto $-4
movwf PORTC
retlw 0
Out4X2: ; ch4=ch6<ch2
movf PWM_4,w
subwf PWM_2
call Start246
nop
movlw (1<<CH_2)
nop
decfsz PWM_4
goto $-4
movwf PORTC
nop
movlw 0
nop
decfsz PWM_2
goto $-4
movwf PORTC
retlw 0
Out642: ; ch6<ch4<ch2
movf PWM_6,w
subwf PWM_4
addwf PWM_4,w
subwf PWM_2
call Start246
nop
movlw (1<<CH_2)|(1<<CH_4)
nop
decfsz PWM_6
goto $-4
movwf PORTC
nop
movlw (1<<CH_2)
nop
decfsz PWM_4
goto $-4
movwf PORTC
nop
movlw 0
nop
decfsz PWM_2
goto $-4
movwf PORTC
retlw 0
Out624: ; ch6<ch2<ch4
movf PWM_6,w
subwf PWM_2
addwf PWM_2,w
subwf PWM_4
call Start246
nop
movlw (1<<CH_2)|(1<<CH_4)
nop
decfsz PWM_6
goto $-4
movwf PORTC
nop
movlw (1<<CH_4)
nop
decfsz PWM_2
goto $-4
movwf PORTC
nop
movlw 0
nop
decfsz PWM_4
goto $-4
movwf PORTC
retlw 0
else
movf PWM_2,w
subwf PWM_4,w
skpnz
goto Out2X
skpc
goto Out42
Out24: ; ch2<ch4
movf PWM_2,w
subwf PWM_4
call Start24
nop
movlw (1<<CH_4)
nop
decfsz PWM_2
goto $-4
movwf PORTC
nop
movlw 0
nop
decfsz PWM_4
goto $-4
movwf PORTC
retlw 0
Out42: ; ch4<ch2
movf PWM_4,w
subwf PWM_2
call Start24
nop
movlw (1<<CH_2)
nop
decfsz PWM_4
goto $-4
movwf PORTC
nop
movlw 0
nop
decfsz PWM_2
goto $-4
movwf PORTC
retlw 0
Out2X ; ch2=ch4
call Start24
nop
movlw 0
nop
decfsz PWM_2
goto $-4
movwf PORTC
retlw 0
endif
;******************************************************************************
; Main
;******************************************************************************
Main: btfsc Flags,WATCH ; did the watchdog timeout ?
goto Failsafe ; oops! try to keep going ...
clrf Flags ; clear all flags
movlw 250 ; wait 500mS for Rx to stabilise
call dx1k ; (signal LED is on)
movlw 250
call dx1k
Start: movlw (1500-750)/6
movwf PPM_1
movwf PPM_2 ; all channels at midpoint
movwf PPM_3
movwf PPM_4
movwf PPM_5
ifdef CH_6
movwf PPM_6
endif
movlw GOODCOUNT ; set number of good frames required
movwf GoodFrames ; before saving failsafe values.
movlw HOLDCOUNT ; set number of bad frames allowed
movwf HoldFrames ; before going to failsafe.
movlw ARMCOUNT ; set number of low throttle frames
movwf ArmFrames ; before arming throttle
goto no_signal
Failsafe: movlw 1
movwf HoldFrames ; stay in failsafe until signal returns
btfss Flags,GOT_FS ; do we have good failsafe values?
goto no_signal
movf FLS_1,W
movwf PWM_1
movf FLS_2,W ; get failsafe values
movwf PWM_2
movf FLS_3,W
movwf PWM_3
movf FLS_4,W
movwf PWM_4
movf FLS_5,W
movwf PWM_5
ifdef CH_6
movf FLS_6,W
movwf PWM_6
endif
call Output ; output failsafe frame
bcf Flags,ARMED ; keep throttle OFF!
movlw ARMCOUNT
movwf ArmFrames ; recharge arming delay
no_signal:
wait_sync: clrwdt ; we're still sane, no reset please!
clrf Temp1
movlw 10 ; set 'gap detect' timeout to 25.6mS
movwf Temp2
wait_gap: SKIP_PPM_HI ; wait for a gap ]
goto time_gap ; ]
nop ; ]
clrwdt ; ]
nop ; ] 10uS per loop
decf Temp1 ; ]
skpnz ; ]
decfsz Temp2 ; timed out ? ]
goto wait_gap ; ]
goto badframe ; can't find sync gap!
time_gap: clrf PPMcount ; reset gap timer
in_gap: decf Temp1 ; }
skpnz ; }
decf Temp2 ; timed out ? }
skpnz ; }
goto badframe ; can't find sync gap! } 10uS per loop
SKIP_PPM_LO ; still in gap ? }
goto wait_gap ; }
incfsz PPMcount ; gap > 2.56mS ? }
goto in_gap ; no, continue timing }
Get_Frame: movlw 128-(23000/256) ; set 23mS timeout
movwf TMR0
wait_1st: clrwdt
btfsc TMR0,7 ; timer reached 128 ?
goto badframe
SKIP_PPM_HI ; wait for start of first channel
goto wait_1st
clrf Channels ; no channels received yet!
NO_OP 4
call GetPPM ; get first channel
addlw -2
skpz
goto badframe
movf PPMcount,W
movwf PPI_1
incf Channels
call GetPPM ; get 2nd channel
addlw -2
skpz
goto badframe
movf PPMcount,W
movwf PPI_2
incf Channels
call GetPPM ; get 3rd channel
addlw -2
skpz
goto badframe
movf PPMcount,W
movwf PPI_3
incf Channels
call GetPPM ; get 4th channel
addlw -2
skpz
goto nochannel
movf PPMcount,W
movwf PPI_4
incf Channels
call GetPPM ; get 5th channel
addlw -2
skpz
goto nochannel
movf PPMcount,W
movwf PPI_5
incf Channels
ifdef CH_6
call GetPPM ; get 6th channel
addlw -2
skpz
goto nochannel
movf PPMcount,W
movwf PPI_6
incf Channels
endif
goto update
nochannel: addlw 1
skpz ; channel timed out?
goto badframe
movlw (1500-750)/6
movwf PPI_4 ; channels 4~6 may not exist!
movwf PPI_5
ifdef CH_6
movwf PPI_6
endif
btfss Flags,GOT_FS ; got failsafe frame yet ?
goto update ; no, ignore missing channels
movf Channels,w
subwf NumChannels ; got correct number of channels ?
skpnz
goto update ; yes
badframe: movlw ARMCOUNT ; reset throttle arming delay
movwf ArmFrames
decfsz HoldFrames ; too many bad frames ?
goto hold
goto Failsafe
hold: btfss Flags,GOT_FS ; good frame available for hold ?
goto no_signal
movf PPA_1,w
movwf PWM_1
movf PPA_2,w
movwf PWM_2
movf PPA_3,w
movwf PWM_3 ; get previous averaged outputs
movf PPA_4,w
movwf PWM_4
movf PPA_5,w
movwf PWM_5
ifdef CH_6
movf PPA_6,w
movwf PWM_6
endif
call Output ; create servo pulses
goto no_signal
;
; Got a good frame. Average the pulse widths of this frame and the last frame.
;
update: movf PPI_1,w
addwf PPM_1,w ; PPA = average(this+last)
movwf PPA_1
rrf PPA_1
movf PPI_2,w
addwf PPM_2,w
movwf PPA_2
rrf PPA_2
movf PPI_3,w
addwf PPM_3,w
movwf PPA_3
rrf PPA_3
movf PPI_4,w
addwf PPM_4,w
movwf PPA_4
rrf PPA_4
movf PPI_5,w
addwf PPM_5,w
movwf PPA_5
rrf PPA_5
ifdef CH_6
movf PPI_6,w
addwf PPM_6,w
movwf PPA_6
rrf PPA_6
endif
movf PPA_1,w
movwf PWM_1 ; PWM = average
movf PPA_2,w
movwf PWM_2
movf PPA_3,w
movwf PWM_3
movf PPA_4,w
movwf PWM_4
movf PPA_5,w
movwf PWM_5
ifdef CH_6
movf PPA_6,w
movwf PWM_6
endif
btfss Flags,GOT_FS ; failsafe frame captured ?
goto save_frame ; no, skip output
ifdef ARM_THROTTLE
btfsc Flags,ARMED ; throttle armed ?
goto do_output ; yes, do normal output
movlw (1300-750)/6
btfsc Flags,JR
subwf PPM_1,w ; throttle < 1.3mS ?
btfss Flags,JR
subwf PWM_3,w
skpc
goto low_throttle ; yes
movlw ARMCOUNT
movwf ArmFrames ; no, recharge arming delay
goto mod_throttle
low_throttle: decfsz ArmFrames ; got enough arming frames ?
goto mod_throttle
bsf Flags,ARMED ; yes, arm the throttle!
goto save_frame
mod_throttle: btfsc Flags,JR
goto mod_jr
movf FLS_3,w
movwf PWM_3 ; keep throttle off until armed
goto do_output
mod_jr: movf FLS_1,w
movwf PWM_1 ; keep JR throttle off until armed
endif
do_output: call Output ; output servo pulses
save_frame: movf PPI_1,W
movwf PPM_1
movf PPI_2,W
movwf PPM_2
movf PPI_3,W
movwf PPM_3 ; remember this frame
movf PPI_4,W
movwf PPM_4
movf PPI_5,W
movwf PPM_5
ifdef CH_6
movf PPI_6,W
movwf PPM_6
endif
movlw HOLDCOUNT ; recharge hold timeout
movwf HoldFrames
btfsc Flags,GOT_FS ; got failsafe frame ?
goto frame_done ; yes
decfsz GoodFrames ; got enough good frames ?
goto frame_done ; no
; get failsafe values
movf Channels,w ; remember number of channels
movwf NumChannels
movf PPA_1,W
movwf FLS_1
movf PPA_2,W
movwf FLS_2
movf PPA_3,W ; copy averaged frame to failsafe
movwf FLS_3
movf PPA_4,W
movwf FLS_4
movf PPA_5,W
movwf FLS_5
ifdef CH_6
movf PPA_6,W
movwf FLS_6
endif
bsf Flags,GOT_FS ; failsafe frame captured
ifdef ARM_THROTTLE
ifdef DETECT_JR
movlw (1300-750)/6
subwf FLS_1,w ; channel 1 < 1.3mS ?
skpc
goto jr_detected
movlw (1700-750)/6
subwf FLS_1,w ; channel 1 >= 1.7mS ?
skpc
goto futaba
movlw (1100-750)/6 ; low throttle in failsafe!
movwf FLS_1
jr_detected: bsf Flags,JR ; JR throttle detected
goto arm
endif ; DETECT_JR
futaba: movlw (1300-750)/6
subwf FLS_3,w ; channel 3 >= 1.3mS ?
skpc
goto arm
movlw (1100-750)/6 ; low throttle in failsafe!
movwf FLS_3
arm: bsf Flags,ARMED ; arm throttle now
endif ; ARM_THROTTLE
frame_done: goto wait_sync ; wait for next frame
;---------- Oscillator Calibration Subroutine --------------
org 0x3ff
ifdef OSCAL_NO
retlw OSCAL_NO
endif
END
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -