list p=16c73A include ; albtrs1.asm ;%%%%%%%%%%%%% line 664 was modified 081401 to generate a variant for 60" ;%%%%%%%%%%%%% slope flw. ; System Overview ; ; Micro-Controller: ; ; Controller Device: PIC16C73A ; Fosc = 4 MHz ; (A/D Tosc x 32 = 4 us (> 1.6 us) - 8 MHz) ; A/D Tosc x 8 = 2 us (> 1.6 us) - 4 MHz ; A/D ref = Vref ; ; ; "FLW" 16C73A PIN ASSIGNMENT: ; ; Pin # Function - New ; ; 1 Pulled up (/MCLR) ; 2 RA0, AN0 (In) - 2.5V ; 3 RA1, AN1 (In) - 2.5V ; 4 RA2, AN2 (In) - 2.5V ; 5 RA3, AN3 (IN) - 2.5V ; 6 RA4, (O.C.) - NU ; 7 RA5, AN4 (In) - AOA ; 8 Vss (GND) ; 9 OSC1 ; 10 OSC2 ; 11 RC0, (IN) - NU ; 12 RC1, (In) - CH5, NU ; 13 RC2, (In) - CH2, AIL ; 14 RC3, (IN) - CH1, ELEV ; ; 15 RC4, (Out) - LEFT WING SERVO ; 16 RC5, (Out) - RIGHT WING SERVO ; 17 RC6/TX, (Out) - NU ; 18 RC7/RX, (Out) - NU ; 19 Vss (GND) ; 20 Vdd (Vcc) ; 21 RB0, (OUT) - MON_DATA ; 22 RB1, (OUT) - MON_DATA ; 23 RB2, (OUT) - MON_DATA ; 24 RB3, (OUT) - MON_DATA ; 25 RB4, (OUT) - MON_DATA ; 26 RB5, (OUT) - MON_DATA ; 27 RB6, (OUT) - MON_DATA ; 28 RB7, (OUT) - MON_DATA ; ;constants ; ADC equ 0x41 ;ADCON0 contrl bits - 4 MHz &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& ;ADC equ 0x81 ;ADCON0 contrl bits - 8 MHz &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& ; ; ;ASM constants, bank 0 ; W equ 0 STATUS equ 3 FSR equ 4 PORTA equ 5 PORTB equ 6 PORTC equ 7 PCLATH equ 0x0a INTCON equ 0x0b PIR1 equ 0x0c PIR2 equ 0x0d RCSTA equ 0x18 TXREG equ 0x19 ADRES equ 0x1e ADCON0 equ 0x1f T1CON equ 0x10 TMR1L equ 0x0e TMR1H equ 0x0f ; ;ASM constants, bank 1 ; TRISA equ 0x85 TRISB equ 0x86 TRISC equ 0x87 PIE1 equ 0x8c PIE2 equ 0x8d TXSTA equ 0x98 SPBRG equ 0x99 ADCON1 equ 0x9f ; ; ;Other constants TMRRPR equ 0x0A ;TMRR preset value ; ; ; ;assigned address locations ; ; ANALG0 equ 0x20 ;AN0 input value, Auxiliary Pitch Input ANALG1 equ 0x21 ;AN1 input value, Static Autopilot LR Input ANALG2 equ 0x22 ;AN2 input value, Static Autopilot RR Input ANALG3 equ 0x23 ;AN3 input value, Static Autopilot Tail Input ANALG4 equ 0x24 ;AN4 input value, AOA Sensor Input ; STATICR equ 0x25 ;Static Roll Input STATICP equ 0x26 ;Static Pitch Input ; MEMAILL equ 0x27 ;remembered AILL MEMAILR equ 0x28 ;remembered AILR ; CH1 equ 0x29 ;Channel 1 (ELEVATOR) input value, Elev CH2 equ 0x2A ;Channel 2 (AILERON) input value, Ail CH3 equ 0x2B ;Channel 3 (THROTTLE) input value, AOA Gain Control CH4 equ 0x2C ;Channel 4 (RUDDER) input value, Store L/R FLAP values CH6 equ 0x2D ;Channel 5 (FLAP) input value, Static Gain Control ; ; TIMOUT equ 0x30 ;timeout reg ADTMR equ 0x31 ;AD timer reg MCNT equ 0x32 ;Multiplication cntr MP equ 0x33 ;Multiplication Product ML equ 0x34 ;Multiplication Multiplicand MI equ 0x35 ;Multiplication Multiplier MLNEG equ 0x36 ;ML negative flag ; TMRR equ 0x37 ;Incr/decr timer register GR0 equ 0x38 ;General purpose reg0 GR1 equ 0x39 ;General purpose reg1 GR2 equ 0x3A ;General purpose reg1 ; AILL equ 0x72 ;Aileron Digital Out Value Reg AILR equ 0x74 ;Aileron Digital Out Value Reg ; ALL equ 0x72 ;Ail Left lsb, unfiltered ALM equ 0x73 ;Ail Left msb, unfiltered ARL equ 0x74 ;Ail Right lsb, unfiltered ARM equ 0x75 ;Ail Right msb, unfiltered ; org 0 ;program start = 0 reset ; goto main ;goto main routine org 4 ;temp start (no interr handling routine req'd) ; main ;main init routine, init system ; ;perform all bank 1 inits bsf STATUS,5;set bank 1 ;preset tristate regs movlw 0x3F ;set w = 3F, includes AOA movwf TRISA ;set tristate reg A movlw 0x0 ;set W = 0x0 movwf TRISB ;set tristate reg B movlw 0xCF ;set W = 0xCF movwf TRISC ;set tristate reg C movlw 0 ;set W = 0 movwf ADCON1 ;and load A/D control reg 1 ;disable all interr ENs clrf PIE2 ;and load periph interr ctrl reg 2 clrf PIE1 ;and load periph interr ctrl reg 1 clrf INTCON ;and load interr ctrl reg clrf 0x81 ;clear option reg (provides weak pullups) ;now do bank 0 inits ;perform all bank 0 inits bcf STATUS,5;set bank 0 clrf PIR1 ;clear interr flags PIR1 clrf PIR2 ;clear interr flags PIR2 clrf PORTA ;clear PORTA clrf PORTB ;clear PORTB clrf PORTC ;clear PORTC ;A/D Control reg 0 movlw ADC ;load w with ctrl bits (0x41) movwf ADCON0 ;and load reg ; movlw 0x80 ;preset W movwf MEMAILL ;and to MEMAILL movwf MEMAILR ;and to MEMAILR ; ;test initialization ^^^^^^^^^^^^^^^^^^test^^^^^^^^^^^^^^^ ;initialization complete^^^^^^^^^^^^^^^^^^end test^^^^^^^^^^^ ; ;***********************************test presets ;digital presets movlw 0x80 ;80 to W movwf CH1 ;and to CH1, elevator movlw 0x80 ;80 to W movwf CH2 ;and to CH2, aileron movlw 0x80 ;80 to W movwf CH3 ;and to CH3, AOA Gain Control (throttle) movlw 0x80 ;80 to W movwf CH4 ;and to CH4, flap memory select (rudder) movlw 0xFF ;80 to W movwf CH6 ;and to CH6, Autopilot/Aux Gain Control (flap) ;analog presets movlw 0x80 ;80 to W movwf ANALG0 ;and to ANALG0 - Aux Pitch Input movlw 0x90 ;80 to W movwf ANALG1 ;and to ANALG1 - Static L Input movlw 0x90 ;80 to W movwf ANALG2 ;and to ANALG2 - Static R Input movlw 0x70 ;80 to W movwf ANALG3 ;and to ANALG3 - Static T Input movlw 0x80 ;80 to W movwf ANALG4 ;and to ANALG4 ;***********************************end test presets ; ; mainloop ;test update mode function select ; ; call subr7 ;call Perform Refresh subroutine goto mainloop;refresh done ; ; subr7 ;Perform Refresh subroutine call readd ;call Read Ch 1, 2, 3 (4 & 6 NU) digital PWM data ;Ch 1: Elevator ;Ch 2: Aileron ;Ch 3: (Throttle) STATIC Gain Control (WAS AOA GAIN CTRL) ;Ch 4: (Rudder) L/R FLAP Position Memory (NU) ;Ch 6: (Flap) Static Autopilot Gain Control (NU) call ad ;call Read Analog Sensor Data subroutine ;modified 081001, invert AOA input (AN4) call gain ;call Gain Ctl - AOA and Static call calc ;call Calculator subroutine call fltrd ;call Digital bounding Output Filter subroutine ; call flpmem ;call flap memory subroutine ;test1 goto test1 ;************************ call writed ;call Write Digital subroutine return ; ; flpmem ;stores current flap positions (left and right) movlw 0xF0 ;set W to max threshold level subwf CH4,W ;subtract W from CH4 btfsc STATUS,0;skip if borrow set (negative result) goto flpmem1 ;branch, result positive, store current flps ; movlw 0x10 ;set W to min threshold level subwf CH4,W ;subtract W from CH4 btfss STATUS,0;skip if borrow cleared (positive result) goto flpmem2 ;branch, result negative, display remembered flps return ;return from flpmem ; flpmem1 ;store current flps movf AILL,W ;AILL to W movwf MEMAILL ;and W to MEMAILL movf AILR,W ;AILL to W movwf MEMAILR ;and W to MEMAILR return ;return from flpmem ; flpmem2 ;display remembered flps movf MEMAILL,W ;MEMAILL to W movwf AILL ;and W to AILL movf MEMAILR,W ;MEMAILL to W movwf AILR ;and W to AILR return ;return from flpmem ; ; calc ;calc subroutine ; ;ANALG0 input value, Auxiliary Pitch Input ;ANALG1 input value, Static Autopilot LR Input ;ANALG2 input value, Static Autopilot RR Input ;ANALG3 input value, Static Autopilot Tail Input ;ANALG4 input value, AOA Sensor Input ; ;CH1, Elevator ;CH2, Aileron ;CH3, AOA Gain Control ;CH4, L/R FLP Memory Control ;CH5, Dive Brake ;CH6, Autopilot Gain Control ; ;ALL PTR = 0x72, Left Flap ;ARL PTR = 0x74, Right Flap ; ;init AL and AR and add elevator (CH1) movf CH1,W ;CH1 to W movwf ALL ;and to ALL movwf ARL ;and to ARL clrf ALM ;clr ALM clrf ARM ;clr ARM ;add / sutract ail input ;left flap first movlw 0x72 ;ALL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf CH2,W ;CH2 (ail) + 0x80 (W) to W call add ;add to AL ; ;right flap next movlw 0x74 ;ARL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf CH2,W ;CH2 (ail) + 0x80 (W) to W movwf GR2 ;and move to GR2 temp reg call invert ;negate ail value movf GR2,W ;negative ail value to W call add ;add to AR ; ;add ANALG4, next AOA value ;left flap first movlw 0x72 ;ALL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf ANALG4,W;ANALG4 + 0x80 to W call add ;add to AL ; ;add ANALG4, next AOA value ;right flap next movlw 0x74 ;ARL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf ANALG4,W;ANALG4 + 0x80 to W call add ;add to AR ; ;add ANALG0, next Aux Pitch value ;left flap first movlw 0x72 ;ALL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf ANALG0,W;ANALG0 + 0x80 to W call add ;add to AL ; ;add ANALG0, next Aux Pitch value ;right flap next movlw 0x74 ;ARL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf ANALG0,W;ANALG0 + 0x80 to W call add ;add to AR ; ;add next Static Autopilot values ;calculate STATICR and STATICP values rrf ANALG3,F;Static T/2 > T bcf ANALG3,7;clear msb rrf ANALG1,F;Static LR/2 > LR bcf ANALG1,7;clear msb rrf ANALG2,F;Static RR/2 > RR bcf ANALG2,7;clear msb ; movf ANALG2,W;RR to W subwf ANALG1,W;LR - RR > W addlw 0x80 ;add offset to W movwf STATICR ;and W to STATICR ; movf ANALG1,W;LR to W addwf ANALG2,F;LR + RR > RR rrf ANALG2,W;rr (LR + RR) > W subwf ANALG3,W;T - (LR + RR)/2 > W movwf GR2 ;and to GR2 btfsc PORTC,6 ;skip if reset (inverse pitch polarity if set) call invert ;invert result movf GR2,W ;result to W addlw 0x80 ;add offset to W movwf STATICP ;and to STATICP ; ;add / sutract STATICR input ;left flap first movlw 0x72 ;ALL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf STATICR,W ;STATICR (ail) + 0x80 (W) to W call add ;add to AL ; ;right flap next movlw 0x74 ;ARL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf STATICR,W ;STATICR (ail) + 0x80 (W) to W movwf GR2 ;and move to GR2 temp reg call invert ;negate ail value movf GR2,W ;negative ail value to W call add ;add to AR ; ;add STATICP (pitch) value ;left flap first movlw 0x72 ;ALL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf STATICP,W ;STATICP + 0x80 to W call add ;add to AL ; ;add ANALG0, next Aux Pitch value ;right flap next movlw 0x74 ;ARL ptr to W movwf FSR ;and to FSR movlw 0x80 ;preset W addwf STATICP,W ;STATICP + 0x80 to W call add ;add to AR ; return ;calc subr return ; ; ; fltrd ;Digital Write Output Filter subroutine ;Input byte pair is specified by ptr in FSR ;Result is left in lsb byte of target byte pair ;Result position range is from 0 to 255. movlw 0x03 ;set output channel count + 1 to W movwf GR0 ;and set ctr reg movlw 0x71 ;preset base - 1 adrs to W movwf FSR ;and load to indirect ptr reg fltrd1 ; incf FSR,F ;incr base adrs, ptr to lsb byte incf FSR,F ;incr base adrs, ptr to msb byte decfsz GR0,F ;test count goto fltrd2 ;continue return ;done, return fltrd2 ; btfss 0,7 ;test msb, skip if neg goto fltrdp ;goto pos processing fltrdn ;perform neg processing decf FSR,F ;decr ptr reg to lsb clrf 0 ;and clear lsb, loads min position value incf FSR,F ;incr ptr reg to msb byte goto fltrd1 ;do next parameter ; fltrdp ;perform pos processing movlw 0xFF ;mask to W andwf 0,F ;and with msb btfss STATUS,2;skip if zero goto fltrdp1 ;overflow goto fltrd1 ;leave lsb alone, do next parameter fltrdp1 ;overflow movlw 0xFF ;set max position value decf FSR,F ;decr ptr reg to lsb movwf 0 ;and load max value to lsb incf FSR,F ;incr ptr reg goto fltrd1 ;do next parameter ; ; ; ad ;Read Analog Sensor Data subroutine ;ANALG0 equ 0x20 ;AN0 input value, Auxiliary Pitch Control ;update APL movlw ADC ;load AD constant to W iorlw 0x00 ;OR in AD chan # 0 movwf ADCON0 ;and update AD constant reg call adtmr ;perform AD aquis timeout bsf ADCON0,2;set AD GO bit adloop0 ; btfsc ADCON0,2;skip if AD conversion done goto adloop0 ;not done, cont ;AD done ; movf ADRES,W ;result to W movwf ANALG0 ;and store result to ANALG0 ; ad1 ; ;ANALG1 equ 0x21 ;AN1 input value, Static Autopilot LR Input movlw ADC ;load AD constant to W iorlw 0x08 ;OR in AD chan # 1 movwf ADCON0 ;and update AD constant reg call adtmr ;perform AD aquis timeout bsf ADCON0,2;set AD GO bit adloop1 ; btfsc ADCON0,2;skip if AD conversion done goto adloop1 ;not done, cont ;AD done ; movf ADRES,W ;result to W movwf ANALG1 ;and store result ANALG1 ; ad2 ; ;ANALG2 equ 0x22 ;AN2 input value, Static Autopilot RR Input movlw ADC ;load AD constant to W iorlw 0x10 ;OR in AD chan # 2 movwf ADCON0 ;and update AD constant reg call adtmr ;perform AD aquis timeout bsf ADCON0,2;set AD GO bit adloop2 ; btfsc ADCON0,2;skip if AD conversion done goto adloop2 ;not done, cont ;AD done ; movf ADRES,W ;result to W movwf ANALG2 ;and store result ANALG2 ; ad3 ; ;ANALG3 equ 0x23 ;AN3 input value, Static Autopilot Tail Input movlw ADC ;load AD constant to W iorlw 0x18 ;OR in AD chan # 3 movwf ADCON0 ;and update AD constant reg call adtmr ;perform AD aquis timeout bsf ADCON0,2;set AD GO bit adloop3 ; btfsc ADCON0,2;skip if AD conversion done goto adloop3 ;not done, cont ;AD done ; movf ADRES,W ;result to W movwf ANALG3 ;and store result ANALG3 ; ad4 ; ;ANALG4 equ 0x24 ;AN4 input value, AOA Sensor Input movlw ADC ;load AD constant to W iorlw 0x20 ;OR in AD chan # 4 movwf ADCON0 ;and update AD constant reg call adtmr ;perform AD aquis timeout bsf ADCON0,2;set AD GO bit adloop4 ; btfsc ADCON0,2;skip if AD conversion done goto adloop4 ;not done, cont ;AD done ; movf ADRES,W ;result to W ;modified 081001 (invert result) xorlw 0xFF ;invert result movwf ANALG4 ;and store result ANALG4 ; return ;Return from AD Subroutine ; gain ;Modified 081001, CH3 controls STATIC gain ;ANALG4 is next AOA processed sensor value to be used ;for flap deflection calculations. ;Modify (multiply by 1 plus incr) ANALG0 by ;CH3 input (hex): ; ; CH3 Range Incr ; 0-255 0-1/2 ; ; ;determine absolute value of delta from neutral ; movf ANALG4,W;ANALG4 to W ; movwf GR2 ;and W to GR2 ; btfss GR2,7 ;skip if positive ; goto gain1 ;goto process neg incr ANALG4 ; ;ANALG4 incr is positive ; bcf GR2,7 ;clear GR2 MSB, GR2 = absol val of ANALG4 ; goto gain2 ;jump ;gain1 ;ANALG4 incr is neg ; movlw 0x80 ;preset W to 80 ; movwf GR1 ;and to GR1 ; movf GR2,W ;GR2 to W ; subwf GR1,W ;GR1-W > W ; movwf GR2 ;W to GR2, GR2 = absol val of ANALG4 ; ; ;gain2 ;multiply GR2 by rr CH3 value ; rrf GR2,F ;right rotate GR2 > F ; bcf GR2,7 ;clear MSB of GR2 ; movf GR2,W ;GR2 > W ; movwf MI ;W > MI ; rrf CH3,F ;right rotate CH3 > F ; bcf CH3,7 ;clear MSB of CH3 ; movf CH3,W ;Ch3 > W ; movwf ML ;W > ML ; call multiply;call multiply subr ; ;subtract MP from ANALG4 ; movf MP,W ;MP > W ; btfss ANALG4,7;skip if ANALG4 positive ; goto gain3 ;ANALG4 negative, jump ; subwf ANALG4,F;ANALG4 positive, ANALG4 = ANALG4 - incr ; goto gain4 ;done ;gain3 ;ANALG4 negative, jump ; addwf ANALG4,F;ANALG4 negative, ANALG4 = ANALG4 + incr ;gain4 ;ANALG4 gain ctl done ; ; ;modified 081001, CH3 will control STATIC gain ;Perform gain control on static autopilot inputs ;ANALG1-3 and also on Aux Pitch input ANALG0. ;CH6 input is gain multiplier value. ; rrf CH3,F ;rr CH6 bcf CH3,7 ;clear CH6 msb movlw 0x20 ;ANALG0 (Aux Pitch Input) ptr to W call rgain ;call rgain (reduced gain subroutine) movlw 0x21 ;ANALG1 (Static LR) ptr to W call rgain ;call rgain (reduced gain subroutine) movlw 0x22 ;ANALG2 (Static RR) ptr to W call rgain ;call rgain (reduced gain subroutine) movlw 0x23 ;ANALG3 (Static Tail) ptr to W call rgain ;call rgain (reduced gain subroutine) ; return ;gain controls done, return ; rgain ;reduced gain control subroutine movwf FSR ;load W to FSR ptr reg movlw 0x80 ;set W subwf 0,W ;[FSR] - W to W movwf ML ;W to ML movf CH3,W ;CH6 to W movwf MI ;and to MI ; call multiply;call multiply subroutine ; ; rlf MP,F ;rl MP > MP ; bcf MP,0 ;clr MP lsb movlw 0x80 ;set W addwf MP,W ;add offset to MP > W movwf 0 ;and W to [FSR] ; return ;rgain update complete, return ; adtmr ;subroutine - perform A/D aquisition timeout (~20 us) movlw 0x14 ;load timeout repeat cnt to W, for 4 MHz movwf ADTMR ;and load A/D timer reg adtmrlp ; decfsz ADTMR,F ;decr ctr and skip if done goto adtmrlp ;cont return ;A/D aquisition timeout complete ; ; ; writed ;Write Digital subroutine ;first count out div 250, then add position inputs. ;Inputs are position values from 0 to 255. ;data ready for PWM output pulse generation ;left ailerons first bsf PORTC,4 ;set AILL output == 1, PWM call timeot ;call 250 count timeout ;divide by I1 lsb count ;generate rudder output first movf AILL,W ;preset countdown value to W movwf TIMOUT ;W to TIMOUT incf TIMOUT,F;incr TIMOUT, to prevent starting with zero write0 nop ;loop start decfsz TIMOUT,F;decr and skip if zero goto write0 ;continue bcf PORTC,4 ;reset AILL output, PWM ;left ailerons done ; ;right ail next bsf PORTC,5 ;set AILR output == 1, PWM call timeot ;call 250 count timeout ;divide by I1 lsb count movf AILR,W ;preset countdown value to W movwf TIMOUT ;W to TIMOUT incf TIMOUT,F;incr TIMOUT, to prevent starting with zero write1 nop ;loop start decfsz TIMOUT,F;decr and skip if zero goto write1 ;continue bcf PORTC,5 ;reset AILL output, PWM ;right ail done ; ;all controls done return ;return from writed subroutine ; readd ;read Ch 1, 2, 3, (4, 6 NU) digital PWM data ;Ch 1: Elevator ;Ch 2: Aileron ;Ch 3: (Throttle) STATIC (AOA NU) Gain Control ;Ch 4: (Rudder) L/R FLAP Position Memory (NU) ;Ch 6: (Flap) Static Autopilot Gain Control (NU) ; ;Ch 1: Elevator clrf CH1 ;clear ch1 ctr, elevator input readd11 btfss PORTC,3 ;test ch1 == 1 goto readd11 ;zero, wait for ch1 == 1 call timeot ;call 250 count timeout readd12 incf CH1,F ;incr ctr btfsc PORTC,3 ;test input goto readd12 ;continue ;done - ch1 ; ;Ch 2: Aileron clrf CH2 ;clear CH2 ctr, aileron input readd21 btfss PORTC,2 ;test ch2 == 1 goto readd21 ;zero, wait for ch3 == 1 call timeot ;call 250 count timeout readd22 incf CH2,F ;incr ctr btfsc PORTC,2 ;test input goto readd22 ;continue ;done - ch2 ; ;%%%%%%%%%%%%% The following %marked lines were added/modified 081401 for ;%%%%%%%%%%%%% 60" flw variant. ;Ch 3: (Throttle) STATIC (AOA) Gain Control ;add STATIC (AOA) gain capability via CH3 clrf CH3 ;clear CH3 ctr, flap input return ;%%%%%%%%%%%%% 081401, added line to bypass CH3 AOA ;%%%%%%%%%%%%% gain control. Disables it. readd31 btfss PORTC,1 ;test ch3 == 1 goto readd31 ;zero, wait for ch3 == 1 call timeot ;call 250 count timeout readd32 incf CH3,F ;incr ctr btfsc PORTC,1 ;test input goto readd32 ;continue ;(modified 081001) ;test for nearly zero gain movf CH3,W ;CH3 to W andlw 0xC0 ;and W & C0 btfss STATUS,2;skip if zero detected return ;return, not zero clrf CH3 ;near zero, force = 0 ;done - ch3 return ;(modified 081001) ; ; ;Ch 4: (Rudder) L/R FLAP Position Memory NU ; ; ; ;add AOA gain capability via CH3 ; clrf CH4 ;clear CH3 ctr, flap input ;readd41 btfss PORTC,7 ;test ch3 == 1 ; goto readd41 ;zero, wait for ch3 == 1 ; call timeot ;call 250 count timeout ;readd42 incf CH4,F ;incr ctr ; btfsc PORTC,7 ;test input ; goto readd42 ;continue ; ;done - Ch4 ; ;Ch 6: (Flap) Static Autopilot Gain Control ; ; ; ;add AOA gain capability via CH3 ; clrf CH6 ;clear CH3 ctr, flap input ;readd61 btfss PORTC,1 ;test ch3 == 1 ; goto readd61 ;zero, wait for ch3 == 1 ; call timeot ;call 250 count timeout ;readd62 incf CH6,F ;incr ctr ; btfsc PORTC,1 ;test input ; goto readd62 ;continue ; ;done - Ch6 ; return ;done - CHs 1, 2, 3, 4, 6 ; timeot ;divide by 250 timeout subroutine movlw 0xf8 ;preset countdown value to W movwf TIMOUT ;W to TIMOUT timlp nop ;loop satrt decfsz TIMOUT,F;decr and skip if zero goto timlp ;continue return ;done, return from timeout ; multiply ;subroutine ;set ML = signed multiplicand (SVVV VVVV) ;set MI = positive multiplier (<=1000 0000) ;Result to MP (SVVV VVVV) ; ;test ML = neg bcf MLNEG,0 ;set pos flag btfss ML,7 ;skip if ML = pos goto mlt00 ;do multiplication ;ML is neg bsf MLNEG,0 ;set neg flag movf ML,W ;ML to W movwf GR2 ;and to GR2 call invert ;invert to pos number movf GR2,W ;GR2 to W movwf ML ;and to ML, do multiplication mlt00 ; rrf MI,F ;shift MI right bcf MI,7 ;clear MI msb bit clrf MCNT ;clr counter reg bsf MCNT,3 ;set to 8 decf MCNT,F ;decr MCNT clrf MP ;clear multiplier results reg movf ML,W ;ML to W goto mlt1 ;start multiplication mlt0 rrf MP,F ;shiftr product MP, test and retain sign bit bcf MP,7 ;clear new sign bit mlt1 ; btfss MI,0 ;bit test MI lsb, skip if set goto mlt2 ;skip addition iteration addwf MP,F ;perform addition mlt2 ; rrf MI,F ;shiftr MI bcf MI,7 ;clear MI msb decfsz MCNT,F ;decr cntr, skip if zero goto mlt0 ;not done, continue ;test neg flag btfss MLNEG,0 ;skip if neg flag set return ;positive, done, return ;neg, invert MP movf MP,W ;MP to W movwf GR2 ;and to GR2 call invert ;invert movf GR2,W ;GR2 to W movwf MP ;and to MP return ;done, return ; add ;signed double precision addition, add W to [*FSR] movwf GR0 ;w to working reg addwf 0,F ;add lsb btfsc STATUS,0;skip if carry = 0 call addc ;carry = 1, goto addc incf FSR,F ;incr ptr btfss GR0,7 ;test sign bit, skip if neg return ;done addneg ;add neg msbs movlw 0xFF ;preset w = -1 addwf 0,F ;add msbs return ;done ; addc ;carry detected incf FSR,F ;incr ptr incf 0,F ;incr msb byte decf FSR,F ;and decr ptr again ; btfss GR0,7 ;test sign bit, skip if neg return ;done ; movlw 0xFF ;preset W = -1 ; addwf 0,F ;add msbs ; return ;done, add subr ; invert ;perform 2's complement on GR2 movlw 0xFF ;set W HI xorwf GR2,F ;complement GR2 incf GR2,F ;incr GR2 return ;done, invert subr ; ; ; end ;