;*************************************************************************** ; MAD3.asm Decoder for Two Magnetic Actuators and Throttle ;*************************************************************************** ; Bruce Abbott bhabbott@paradise.net.nz ; ; for Microchip PIC12C5xx/12F6xx running at @ 4MHz (INT RC Clock) ; ;=========================================================================== ; Description ; ; Decodes PPM input into PWM outputs, giving proportional control of two ; magnetic actuators and throttle. ; ; ; ========================================================================== ; Summary of Changes ; ; 2004/2/2 V0.01 ; ; -------------------------------------------------------------------------- ; #DEFINE version 0 #DEFINE revision 1 ; PROCESSOR PIC12C508 ; PROCESSOR PIC12F675 ifdef __12C508 INCLUDE __CONFIG _MCLRE_OFF&_CP_OFF&_WDT_ON&_IntRC_OSC else INCLUDE __CONFIG _MCLRE_OFF&_CP_OFF&_WDT_ON&_BODEN_ON&_INTRC_OSC_NOCLKOUT endif radix dec errorlevel 0,-305,-302 ;#DEFINE OSCCAL_NO 0x70 ; enable if OSCCAL value was erased! ; Bit definitions for the GPIO register and the TRIS register #DEFINE MOTOR 0 ; pin 7 Motor PWM #DEFINE LEFT 1 ; pin 6 Left PWM out #DEFINE RIGHT 2 ; pin 5 Right PWM out #DEFINE PPM_in 3 ; pin 4 input pulse stream #DEFINE DOWN 4 ; pin 3 Down PWM out #DEFINE UP 5 ; pin 2 Up PWM out #DEFINE TrisBits (1<1 goto $+1 ; 2 clocks NO_OP_COUNT SET NO_OP_COUNT-2 ENDW IF NO_OP_COUNT nop ; 1 clock ENDIF ENDM ;----------------------------------------------------------------------- ; Generate PWM output ;----------------------------------------------------------------------- ; ; Takes 20uS (including call) ; ; Must be called every 26uS. ; ; ; DoPWM: decfsz pwm_period ;3 end of PWM cycle ? goto pwm_step ;4(5) no, do the next step movf Left_PWM,w ;5 yes, movwf left_count ;6 movf Right_PWM,w ;7 movwf right_count ;8 movf Up_PWM,w ;9 reload all counters movwf up_count ;10 movf Down_PWM,w ;11 movwf down_count ;12 movf Motor_PWM,w ;13 movwf motor_count ;14 movlw PWMSTEPS ;15 movwf pwm_period ;16 movf Start_PWM,w ;17 restart active PWMs movwf GPIO ;18 retlw 0 ;19,20 pwm_step: movlw 0 ;6 Assume all PWMs will be OFF decfsz left_count ;7 Count down. If count<>0 then iorlw 1< 2.5mS ? goto time_gap1 ;4(5) no, continue timing goto wait_1st ;5,6 yes, sync gap detected loop_1st: clrwdt ;4 reset watchdog NO_OP 2 ;5,6 wait_1st: call DoPWM btfss GPIO,PPM_in ;1 wait for start of first channel goto loop_1st ;2(3) call GetPPM ;3 get first channel movf temp_1,w ;4 movwf Rudder ;5 nop ;6 call DoPWM NO_OP 2 ;1,2 call GetPPM ;3 get 2nd channel movf temp_1,w ;4 movwf Elevator ;5 nop ;6 call DoPWM NO_OP 2 ;1,2 call GetPPM ;3 get 3rd channel movf temp_1,w ;4 movwf Throttle ;5 nop ;6 call DoPWM goto check ;1,2 badframe: call DoPWM decfsz HoldFrames ;1 too many bad frames ? goto wait_sync ;2(3) no Failsafe: movlw 1 ; movwf HoldFrames ; Stay in failsafe clrf Start_PWM ; clrf GPIO ; Stop all PWM now! goto wait_sync ; ; ; Error if channel width out of range. ; check: movlw 750/26 ;3 subwf Rudder,w ;4 skpc ;5 < 0.75mS ? goto badframe ;6 too short call DoPWM nop ;1 movlw 750/26 ;2 subwf Elevator,w ;3 skpc ;4 goto badframe ;5(6) nop ;6 call DoPWM nop ;1 movlw 750/26 ;2 subwf Throttle,w ;3 skpc ;4 goto badframe ;5(6) nop ;6 call DoPWM nop ;1 movlw 2300/26 ;2 subwf Rudder,w ;3 skpnc ;4 > 2.3mS ? goto badframe ;5(6) too long nop ;6 call DoPWM nop ;1 movlw 2300/26 ;2 subwf Elevator,w ;3 skpnc ;4 goto badframe ;5(6) nop ;6 call DoPWM nop ;1 movlw 2300/26 ;2 subwf Throttle,w ;3 skpnc ;4 goto badframe ;5(6) nop ;6 call DoPWM ; ; output = average ( current , previous ) ; movf Rudder,w ;1 movwf temp_1 ;2 movf PMM_1,w ;3 addwf Rudder ;4 rrf Rudder ;5 nop ;6 call DoPWM movf Elevator,w ;1 movwf temp_2 ;2 movf PMM_2,w ;3 addwf Elevator ;4 rrf Elevator ;5 nop ;6 call DoPWM movf Throttle,w ;1 movwf temp_3 ;2 movf PMM_3,w ;3 addwf Throttle ;4 rrf Throttle ;5 nop ;6 call DoPWM ; remember the current frame. movf temp_1,W ;1 movwf PMM_1 ;2 movf temp_2,W ;3 movwf PMM_2 ;4 movf temp_3,W ;5 movwf PMM_3 ;6 call DoPWM ; ; We have a good frame! ; movlw HOLDCOUNT ;1 reset failsafe timeout movwf HoldFrames ;2 NO_OP 4 ;3~6 call DoPWM nop ;1 btfsc Flags,STARTED ;2 done startup yet ? goto update ;3(4) yes nop ;6 call DoPWM NO_OP 2 ;1,2 ; ; Don't start until several consecutive good frames have been detected. ; decfsz GoodFrames ;3 got enough good frames ? goto frame_done ;5(6) no, so don't update PWMs bsf Flags,STARTED ;6 yes call DoPWM ; ; Detect channel allocations. JR and GWS transmit throttle on channel 1, ; others transmit throttle on channel 3. Channel 1 is assumed to be ; throttle if less than 1.25mS at startup. ; movlw 1250/26 ;1 subwf Rudder,w ;2 channel 1 < 1.25mS ? skpc ;3 bsf Flags,JR ;4 JR/GWS TX (throttle on channel 1) ; ; If JR throttle detected, swap channels ; update: btfsc Flags,JR ;4 JR channel order ? goto copyjr ;5(6) yes nop ;6 no call DoPWM NO_OP 4 ;1~4 goto limit ;5,6 copyjr: call DoPWM movf Rudder,w ;1 movwf temp_1 ;2 temp_1 = CH1 (JR throttle) movf Elevator,w ;3 movwf Rudder ;4 rudder = CH2 (JR rudder) movf Throttle,w ;5 movwf Elevator ;6 elevator = CH3 (JR elevator) call DoPWM movf temp_1,w ;1 movwf Throttle ;2 throttle = CH1 (JR throttle) NO_OP 4 ;3~6 ; ; limit range to 1.1~1.9mS (rudder,elevator=0~31, throttle=0~15) ; limit: call DoPWM movlw 1100/26 ;1 subwf Rudder ;2 skpc ;3 clrf Rudder ;4 1.1mS minimum NO_OP 2 ;5,6 call DoPWM movlw 1100/26 ;1 subwf Elevator ;2 skpc ;3 clrf Elevator ;4 1.1mS minimum NO_OP 2 ;5,6 call DoPWM movlw 1100/26 ;1 subwf Throttle ;2 skpc ;3 clrf Throttle ;4 1.1mS minimum bcf STATUS,C ;5 rrf Throttle ;6 throttle values = 0~15+ call DoPWM movlw 15 ;1 subwf Throttle,w ;2 movlw 15 ;3 skpnc ;4 movwf Throttle ;5 throttle max. value = 15 nop ;6 call DoPWM movf Throttle,w ;1 call Expo ;2~7 apply exponential throttle curve movwf Throttle ;8 call DoPWM ; this PWM is 2uS too late! NO_OP 4 ;3~6 but next PWM will be on time ; ; wait until the next PWM cycle starts. ; wait_pwm: call DoPWM decf pwm_period,w ;1 skpnz ;2 goto last_pwm ;3(4) nop ;4 goto wait_pwm ;5,6 last_pwm: NO_OP 2 ;5,6 call DoPWM ; ; ; Update PWM variables with new values, whilst completing the current PWM ; cycle with old values. ; ; split rudder and elevator into bi-directional PWM ; ; 1~15 = low PWM = 15~1 ; 16 = center PWM = 0 ; 17~31 = high PWM = 1~15 ; movlw 16 ;1 subwf Rudder,w ;2 right or left ? skpc ;3 goto do_left ;4(5) do_right: movwf Right_PWM ;5 right PWM = 0~15 clrf Left_PWM ;6 left PWM = 0 call DoPWM NO_OP 3 ;1~3 goto do_elevator ;5,6 do_left: clrf Right_PWM ;6 right PWM = 0 call DoPWM movlw 16 ;1 movwf Left_PWM ;2 movf Rudder,w ;3 left PWM = 16~1 subwf Left_PWM ;4 NO_OP 2 ;5,6 do_elevator: call DoPWM movlw 16 ;1 subwf Elevator,w ;2 skpc ;3 goto do_down ;4(5) do_up: movwf Up_PWM ;5 clrf Down_PWM ;6 call DoPWM nop ;1 goto do_throttle ;2,3 do_down: clrf Up_PWM ;6 call DoPWM movlw 16 ;1 movwf Down_PWM ;2 movf Elevator,w ;3 subwf Down_PWM ;4 NO_OP 2 ;5,6 call DoPWM NO_OP 3 ;1~3 ; ; get throttle PWM ; do_throttle: movf Throttle,w ;4 movwf Motor_PWM ;5 ; ; Set starting state for each PWM output. ; clrf Start_PWM ;6 Assume all PWM outputs are OFF call DoPWM tstf Left_PWM ;1 PWM count = 0 ? skpz ;2 bsf Start_PWM,LEFT ;3 no, PWM output will be ON tstf Right_PWM ;4 skpz ;5 bsf Start_PWM,RIGHT ;6 call DoPWM tstf Down_PWM ;1 skpz ;2 bsf Start_PWM,DOWN ;3 tstf Up_PWM ;4 skpz ;5 bsf Start_PWM,UP ;6 call DoPWM tstf Motor_PWM ;1 skpz ;2 bsf Start_PWM,MOTOR ;3 NO_OP 3 ;4~6 frame_done: call DoPWM nop ;1 goto wait_sync ;2,3 wait for next frame ;---------- Oscillator Calibration Subroutine -------------- ifdef __12F675 ifdef OSCCAL_NO org 0x3ff retlw OSCCAL_NO endif endif END