I think this will work.
changed last label to Failsafeb
Code:
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 Failsafeb
then try this directly above the routine Failsafe
Code:
Failsafeb:
bsf GPIO,CH_1
bsf GPIO,CH_2
bsf GPIO,CH_3
bsf GPIO,CH_4
movlw 1 ;total 1.5ms
call Servo_Delay
movlw 1
call Servo_Delay
;goto Failsafe ; not needed because execution will continue to Failsafe
I can’t guess if you want to turn throttle off before or after the 1.5ms,
or when you want to reset throttle arming delay which was done in Failsafe.
That turns all outputs on 1 instruction apart.
If you meant cycle them all with 1.5 ms delay each, then ignore all of the above,
and simply write your own delay constants in the original routine instead of copying
from the variables.
This flows onto the next label int he program so must be left in place.
Code:
Failsafe: movlw 1
movwf HoldFrames ; stay in failsafe until signal returns
btfss Flags,GOT_FS ; do we have good failsafe values?
goto no_signal
movlw 110 ; only guessing approx value for 1.5ms here
movwf PWM_1
movlw 110
movwf PWM_2
movlw 110
movwf PWM_3
movlw 110
movwf PWM_4
call Output ; output failsafe frame
bcf Flags,ARMED ; keep throttle OFF
movlw ARMCOUNT
movwf ArmFrames ; reset throttle arming delay
Bookmarks