;-------------------------------------------------------------------------------
; Millisecond Delay Timer
;-------------------------------------------------------------------------------
; Input: W = number of milliseconds to wait (max 256mS)
;
dx1k: movwf Temp1
_dx1k1: movlw (1000-5)/5
movwf Temp2
_dx1k2: clrwdt ; avoid watchdog timeout
nop
decfsz Temp2 ; wait 1mS
goto _dx1k2
decfsz Temp1
goto _dx1k1
retlw 0
;*******************************************************************************
; 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: bsf GPIO,LED ; signal LED off
movlw (1500-750)/6
movwf PMM_1
movwf PMM_2 ; all channel outputs at midpoint
movwf PMM_3
movwf PMM_4
movwf PPM_4 ; init channel 4 (for 3 channel TX)
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
call Output ; output failsafe frame
bcf Flags,ARMED ; keep throttle OFF
movlw ARMCOUNT
movwf ArmFrames ; reset throttle arming delay
no_signal: bsf GPIO,LED ; signal LED off
wait_sync: clrwdt ; we're still sane, no reset please!
clrf Temp1
movlw 9 ; set 'gap detect' timeout to 23mS
movwf Temp2
wait_gap: skip_PPM_high ; 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_low ; still in gap ? }
goto wait_gap ; }
incfsz PPMcount ; gap > 2.56mS ? }
goto in_gap ; no, continue timing }
Get_Frame: movlw 128-(23000/256) ; set frame timeout to 23mS
movwf TMR0
wait_1st: clrwdt
btfsc TMR0,7 ; timer reached 23mS ?
goto badframe
skip_PPM_high ; wait for start of first channel
goto wait_1st
bcf Flags,GOT_4 ; channel 4 not received yet
NO_OP 3
call GetPPM ; get 1st channel
andlw 255
skpz ; process return code
goto badframe
movf PPMcount,W
movwf DUMY_1
call GetPPM ; get 2nd channel
andlw 255
skpz ; process return code
goto badframe
movf PPMcount,W
movwf DUMY_1
call GetPPM ; get 3rd channel
andlw 255
skpz ; process return code
goto badframe
movf PPMcount,W
movwf DUMY_1
call GetPPM ; get 4th channel
andlw 255
skpz ; process return code
goto badframe
movf PPMcount,W
movwf DUMY_1
call GetPPM ; get 5th channel
andlw 255
skpz ; process return code
goto badframe
movf PPMcount,W
movwf PPM_1
call GetPPM ; get 6th channel
andlw 255
skpz
goto badframe
movf PPMcount,W
movwf PPM_2
call GetPPM ; get 7th channel
andlw 255
skpz
goto badframe
movf PPMcount,W
movwf PPM_3
call GetPPM ; get 8th channel
andlw 255
skpz
goto error4
movf PPMcount,W
movwf PPM_4
bsf Flags,GOT_4
goto update
error4:
xorlw 2
skpz ; channel 4 missing ?
goto badframe
btfss Flags,GOT_FS ; got failsafe frame yet ?
goto update ; no, ignore missing channel 4
btfss Flags,DET_4 ; 4 channels detected ?
goto update ; no, ignore missing channel 4
badframe: clrwdt
movlw 128-((23000-9000)/256); timer reached 9mS ?
subwf TMR0,w
skpc ; wait 9mS to skip other channels
goto badframe
movlw ARMCOUNT ; reset throttle arming delay
movwf ArmFrames
decfsz HoldFrames ; too many bad frames ?
goto hold
goto Failsafe
hold:
btfsc Flags,GOT_FS ; good frame available for hold ?
call Output ; yes, output last good frame
goto no_signal ; no, just stay silent
; Got a good frame. Output the averaged pulse widths of this frame
; and the last frame.
;
update:
movf PPM_1,W
addwf PMM_1 ; PWM out = average(this+last)
rrf PMM_1,w
movwf PWM_1
movf PPM_2,W
addwf PMM_2
rrf PMM_2,w
movwf PWM_2
movf PPM_3,W
addwf PMM_3
rrf PMM_3,w
movwf PWM_3
movf PPM_4,W
addwf PMM_4
rrf PMM_4,w
movwf PWM_4
btfss Flags,GOT_FS ; no output until failsafe captured
goto output_done
ifdef ARM_THROTTLE
btfsc Flags,ARMED ; throttle armed ?
goto do_output ; yes
movlw (1300-750)/6
btfsc Flags,JR
subwf PWM_1,w ; throttle < 1.3mS ?
btfss Flags,JR
subwf PWM_3,w
skpc
goto low_throttle ; yes
movlw ARMCOUNT
movwf ArmFrames ; no, reset arming delay
goto disarm
low_throttle:
decfsz ArmFrames ; got enough arming frames ?
goto disarm
bsf Flags,ARMED ; yes, arm throttle now
goto do_output
disarm:
movf FLS_3,w
btfss Flags,JR
movwf PWM_3 ; set throttle to failsafe value
movf FLS_1,w
btfsc Flags,JR
movwf PWM_1
endif ; ARM_THROTTLE
do_output:
call Output ; output good frame
output_done:
movf PPM_1,W ; receive input_1-4
movwf PMM_1 ; put to memery_1-4
movf PPM_2,W ; remember all frame
movwf PMM_2
movf PPM_3,W
movwf PMM_3
movf PPM_4,W
movwf PMM_4
movlw HOLDCOUNT ; reset failsafe timeout
movwf HoldFrames
bcf GPIO,LED ; signal LED on
btfsc Flags,GOT_FS ; already got failsafe frame ?
goto frame_done ; yes
clrwdt
decfsz GoodFrames ; got enough good frames ?
goto frame_done ; no
; Got enough good frames, now get failsafe values
btfsc Flags,GOT_4 ; channel 4 detected ?
bsf Flags,DET_4
movf PWM_1,W
movwf FLS_1
movf PWM_2,W
movwf FLS_2
movf PWM_3,W ; copy good output to failsafe
movwf FLS_3
movf PWM_4,W
movwf FLS_4
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 ; failsafe low throttle!
; movwf FLS_1
;
;jr_detected: bsf Flags,JR ; JR throttle detected
; goto arm
; endif ; DETECT_JR
;
;futaba: movlw (1700-750)/6
; subwf FLS_3,w ; channel 3 >= 1.7mS ?
; skpc
; goto arm
; movlw (1100-750)/6 ; failsafe low throttle!
; 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 (12F629/75 only) --------------
ifdef __12F675
org 0x3ff
retlw 0x90 ; replace with oscal value for your PIC!
endif
END |