;****************************************************************************** ; RX A7105 FlySky protocol PIC16F1824 6CH * ; PPM output on channel 6 option * ;****************************************************************************** ; * ; Filename: xxx.asm * ; Date: * ; File Version: * ; * ; Author: Thierry Pébayle * ; Company: * ; * ; * ;****************************************************************************** ; * ; Files Required: P16F1827.INC * ; * ;****************************************************************************** ; * ; Notes: * ; * ;****************************************************************************** ; * ; Revision History: * ; * ;****************************************************************************** ;#define PPM ;uncomment to have PPM output on channel 6 !!! list p=16f1824 ; list directive to define processor #include ; processor specific variable definitions ;------------------------------------------------------------------------------ ; ; CONFIGURATION WORD SETUP ; ; The 'CONFIG' directive is used to embed the configuration word within the ; .asm file. The lables following the directive are located in the respective ; .inc file. See the data sheet for additional information on configuration ; word settings. ; ;------------------------------------------------------------------------------ __CONFIG _CONFIG1, _FOSC_INTOSC & _WDTE_OFF & _PWRTE_OFF & _MCLRE_ON & _CP_OFF & _CPD_OFF & _BOREN_OFF & _CLKOUTEN_OFF & _IESO_OFF & _FCMEN_OFF __CONFIG _CONFIG2, _WRT_OFF & _PLLEN_OFF & _STVREN_OFF & _BORV_19 & _LVP_OFF ;****************************************************************************** ; CONSTANTES ;****************************************************************************** ;LED #define LED PORTC, 3 #define LED_TRIS TRISC, 3 ;com spi A7105 #define CS_SPI PORTA,4 ;chip select SPI #define SCK_SPI PORTA,5 ;serial clock SPI #define SDIO_SPI PORTC,5 ;serial data input/output SPI #define GIO PORTA,2 ;GIO output A7105 #define CS_SPI_TRIS TRISA,4 ;chip select SPI #define SCK_SPI_TRIS TRISA,5 ;serial clock SPI #define SDIO_SPI_TRIS TRISC,5 ;serial data input/output SPI #define GIO_TRIS TRISA,2 ;GIO output A7105 ;servo outputs #define SERVO1 PORTA, 0 #define SERVO2 PORTC, 1 #define SERVO3 PORTC, 2 #define SERVO4 PORTC, 0 #define SERVO5 PORTA, 1 #define SERVO6 PORTC, 4 #define SERVO1_TRIS TRISA, 0 #define SERVO2_TRIS TRISC, 1 #define SERVO3_TRIS TRISC, 2 #define SERVO4_TRIS TRISC, 0 #define SERVO5_TRIS TRISA, 1 #define SERVO6_TRIS TRISC, 4 ;****************************************************************************** ; MACRO ;****************************************************************************** ;compare w and register, skip if equal cpfseq MACRO reg subwf reg, w btfss STATUS, Z ENDM ;move register into register through W ;register1 -> W -> register2 movff MACRO reg1, reg2 movf reg1, w movwf reg2 ENDM ;move literal into register through W ;literal -> W -> register movlwf MACRO valeur, reg movlw valeur movwf reg ENDM initFSR0 macro adrReg banksel FSR0H movlw high adrReg movwf FSR0H movlw low adrReg movwf FSR0L endm initFSR1 macro adrReg banksel FSR1H movlw high adrReg movwf FSR1H movlw low adrReg movwf FSR1L endm ;start timer4 for N us count before overflow ;resolution = 40us = 1/4us * 16(prescaler) * 10(postscaler) ;max duration = 255*40us = 10200us startTimer4Nus MACRO duration banksel T4CON bcf T4CON, TMR4ON ;stops timer4 movlw duration/40-1 ;set comparator register movwf PR4 clrf TMR4 banksel PIR3 bcf PIR3, TMR4IF ;clear timer4 flag banksel T4CON bsf T4CON, TMR4ON ;starts timer4 ENDM ;enable/disable SPI (CS) SPI_ON MACRO banksel PORTA bcf CS_SPI ;enable SPI A7105 ENDM SPI_OFF MACRO banksel PORTA bsf CS_SPI ;disable SPI A7105 ENDM ;one clock pulse for spi com SCK_SPI_PULSE MACRO ;one clock (rising edge then falling edge) banksel PORTA bsf SCK_SPI bcf SCK_SPI ENDM ;SPI write MACRO (adr = register, val = register) spi_write_fadr_fval MACRO fadr, fval SPI_ON movff fadr, spiReg call spi_write movff fval, spiReg call spi_write SPI_OFF ENDM ;SPI write MACRO (adr = register, val = literal) spi_write_fadr_lval MACRO fadr, lval SPI_ON movff fadr, spiReg call spi_write movlw lval movwf spiReg call spi_write SPI_OFF ENDM ;SPI write MACRO (adr = literal, val = register) spi_write_ladr_fval MACRO ladr, fval SPI_ON movlw ladr movwf spiReg call spi_write movff fval, spiReg call spi_write SPI_OFF ENDM ;SPI write MACRO (adr = literal, val = literal) spi_write_ladr_lval MACRO ladr, lval SPI_ON movlw ladr movwf spiReg call spi_write movlw lval movwf spiReg call spi_write SPI_OFF ENDM ;SPI write strobe command (lval = literal) spi_write_strobe_command MACRO lval SPI_ON movlw lval movwf spiReg call spi_write SPI_OFF ENDM ;SPI read MACRO (adr = registers) spi_read_fadr MACRO fadr SPI_ON movff fadr, spiReg bsf spiReg, 6 call spi_write nop call spi_read SPI_OFF ENDM ;SPI read MACRO (adr = literal) spi_read_ladr MACRO ladr SPI_ON movlw ladr movwf spiReg bsf spiReg, 6 call spi_write nop call spi_read SPI_OFF ENDM ;restore FSR0 et FSR1 après interrupt restoreFSR01 MACRO movf FSR0L, w ;FSR0L -> FSR0L_SHAD banksel BSR_SHAD movwf FSR0L_SHAD banksel BSR ;FSR0H -> FSR0H_SHAD movf FSR0H, w banksel BSR_SHAD movwf FSR0H_SHAD banksel BSR ;FSR1L -> FSR1L_SHAD movf FSR1L, w banksel BSR_SHAD movwf FSR1L_SHAD banksel BSR ;FSR1H -> FSR1H_SHAD movf FSR1H, w banksel BSR_SHAD movwf FSR1H_SHAD banksel BSR ENDM ;inverse signe de a (5cy) NEG MACRO aHI, aLO comf aLO, f comf aHI, f incf aLO, f btfsc STATUS, C incf aHI, f ENDM ;ajoute a et b, resultat dans b (6cy) ADD MACRO aHI, aLO, bHI, bLO movf aLO, w addwf bLO, f movf aHI, w addwfc bHI, f ENDM ;****************************************************************************** ; VARIABLES ;****************************************************************************** CBLOCK 0x20 ; Sample GPR variable reg allocations ;init A7105 cptReg ;spi_write, spi_read cptBit, spiReg, adressReg, valReg ;write_eeprom_pic, read_eeprom_pic adr_pic, data_pic ;global variables channel ;rx channel no cptTimeOut ;time out counter while waiting for valid bind packed id0, id1, id2, id3 ;TX id found in current RX packet txid0, txid1, txid2, txid3 ;TX id (current bind or previous bind) ip1h, ip1l ;Input Pulse duration for servo 1 to 6 (high,low) ip2h, ip2l ; = pulse duration read in current RX packet in us ip3h, ip3l ip4h, ip4l ip5h, ip5l ip6h, ip6l ip7h, ip7l ip8h, ip8l op1h, op1l ;Output Pulse duration for servo 1 to 6 (high,low) op2h, op2l ; = servo output pulse duration in us op3h, op3l op4h, op4l op5h, op5l op6h, op6l cptChannel ;RX channel sequence counter (goes from 16 to 1) tableOffset ;offset sequence in tableSequence channelOffset ;channel offset (to be subtract to channel number) ;mask to set or unset servo output on portb & portc ;servo0 does not exist but it simplifies algorythm... maskServo0Porta, maskServo0Portc maskServo1Porta, maskServo1Portc maskServo2Porta, maskServo2Portc maskServo3Porta, maskServo3Portc maskServo4Porta, maskServo4Portc maskServo5Porta, maskServo5Portc maskServo6Porta, maskServo6Portc cptItTimer1 ;timer1 it counter cptItTimer2 ;timer2 it counter tableAdress ;for table read using BRW and RETLW ENDC ;------------------------------------------------------------------------------ ; EEPROM INITIALIZATION ; ; The 16F1827 has 256 bytes of non-volatile EEPROM, starting at address 0xF000 ; ;------------------------------------------------------------------------------ DATAEE ORG 0xF000 DE "MCHP" ; Place 'M' 'C' 'H' 'P' at address 0,1,2,3 ;------------------------------------------------------------------------------ ; RESET VECTOR ;------------------------------------------------------------------------------ ORG 0x0000 ; processor reset vector PAGESEL START GOTO START ; When using debug header, first inst. ; may be passed over by ICD2. ;------------------------------------------------------------------------------ ; INTERRUPT SERVICE ROUTINE ;------------------------------------------------------------------------------ ORG 0x0004 ;------------------------------------------------------------------------------ ; USER INTERRUPT SERVICE ROUTINE GOES HERE ;------------------------------------------------------------------------------ ; Note the 16F1827 family automatically handles context restoration for ; W, STATUS, BSR, FSR, and PCLATH where previous templates for 16F families ; required manual restoration. Shadow registers store these SFR values, and ; shadow registers may be modified since they are readable and writable for ; modification to the context restoration. ; timer2 starts pulse sequence(50Hz), then timer1 counts pulse duration for ; each servo, and timer6 counts 400us PPM pulse ;test timer2 interrupt or timer1 interrupt ? banksel PIR1 btfss PIR1, TMR2IF #ifdef PPM goto timer1_or_timer6_isr #else goto timer1_isr #endif timer2_isr ;(100hz) bcf PIR1, TMR2IF ;clear timer2 it flag incf cptItTimer2, f ;incremente timer2 it counter btfsc cptItTimer2, 0 ;skip if odd RETFIE ;quit interrupt, automatic context saving (W, STATUS, BSR) ;start_pulse_sequence (50hz) initFSR0 op1h ;load pulse duration servo1 (high byte) into indirect register FSR0 initFSR1 maskServo0Porta ;load mask servo0 into indirect register FSR1 movlwf .7, cptItTimer1 ;init timer1 it counter bcf T1CON, TMR1ON ;timer1 off movlwf 0xFF, TMR1H ;timer1 = 0xFFFF => timer1 overflow immediatly movlwf 0xFF, TMR1L ; bsf T1CON, TMR1ON ;timer1 on restoreFSR01 RETFIE ;quit interrupt, automatic context saving #ifdef PPM timer1_or_timer6_isr btfss PIR3, TMR6IF goto timer1_isr timer6_isr bcf SERVO6 ;falling edge PPM pulse bcf PIR3, TMR6IF ;clear timer6 it flag banksel T6CON bcf T6CON, TMR6ON ;stops timer 6 banksel PORTA RETFIE FAST ;quit interrupt, automatic context saving (W, STATUS, BSR) #endif timer1_isr moviw FSR1++ ;stop previous pulse porta xorlw 0xFF andwf LATA, f moviw FSR1++ ;stop previous pulse portc xorlw 0xFF andwf LATC, f bcf T1CON, TMR1ON ;timer1 off bcf PIR1, TMR1IF ;clear timer1 it flag decf cptItTimer1, f ;decremente timer1 it counter btfsc STATUS, Z ;timer1 = 0 ? RETFIE ;yes -> quit interrupt, automatic context saving moviw FSR0++ ;set timer1 duration High byte xorlw 0xFF movwf TMR1H moviw FSR0++ ;set timer1 duration Low byte xorlw 0xFF movwf TMR1L bsf T1CON, TMR1ON ;timer1 on #ifdef PPM bsf SERVO6 ;starts 400us PPM pulse banksel T6CON bsf T6CON, TMR6ON ;starts timer 6 banksel PORTA #endif moviw FSR1++ ;start next pulse porta iorwf LATA, f moviw FSR1-- ;start next pulse portc iorwf LATC, f restoreFSR01 RETFIE ;quit interrupt, automatic context saving ;****************************************************** ;write_eeprom_pic ;****************************************************** write_eeprom_pic banksel EECON1 ;bank 3 bcf EECON1, CFGS ;accès à EEPROM ou program bcf EECON1, EEPGD ;accès à EEPROM banksel PIR2 ;bank 0 bcf PIR2, EEIF ;efface flag fin d'écriture movf data_pic, w banksel EECON1 ;bank 3 movwf EEDATL banksel PORTA ;bank 0 movf adr_pic, w banksel EECON1 ;bank 3 movwf EEADR bsf EECON1, WREN ;Enable write movlw 0x55 ;Unlock write movwf EECON2 ; movlw 0xAA ; movwf EECON2 ; bsf EECON1, WR ;Start the write ;attente fin ecriture banksel PIR2 ;bank 0 attenteFinEcriture btfss PIR2, EEIF goto attenteFinEcriture banksel EECON1 bcf EECON1, WREN ;désactive write banksel PORTA return ;****************************************************** ;read_eeprom_pic ;****************************************************** read_eeprom_pic banksel EECON1 ;bank 3 bcf EECON1, CFGS ;accès à EEPROM ou program bcf EECON1, EEPGD ;accès à EEPROM banksel PORTA ;bank 0 movf adr_pic, w banksel EECON1 ;bank 3 movwf EEADR bsf EECON1, RD ;EE Read movf EEDATL, w banksel PORTA ;bank 0 movwf data_pic ;Move data to data_pic return ;**************************************** ; spi_write ; entrée: ; spiReg: valeur du registre à écrire ;**************************************** spi_write movlwf .8, cptBit banksel PORTA boucleWriteReg btfss spiReg, 7 bcf SDIO_SPI btfsc spiReg, 7 bsf SDIO_SPI nop SCK_SPI_PULSE rlf spiReg, f decfsz cptBit, f goto boucleWriteReg; return ;********************************* ; spi_read ; sortie: ; spiReg: valeur du registre lu ;********************************* spi_read banksel TRISA bsf SDIO_SPI_TRIS ;SDIO input banksel PORTA movlwf .8, cptBit clrf spiReg bcf STATUS, C boucleReadReg rlf spiReg, f btfsc SDIO_SPI bsf spiReg, 0 SCK_SPI_PULSE decfsz cptBit, f goto boucleReadReg banksel TRISA bcf SDIO_SPI_TRIS ;SDIO output banksel PORTA return ;************** ; read_packet ;************** read_packet SPI_ON movlwf 0x45, spiReg call spi_write call spi_read ;sync call spi_read ;id0 TX (lsb) movff spiReg, id0 call spi_read ;id1 TX movff spiReg, id1 call spi_read ;id2 TX movff spiReg, id2 call spi_read ;id3 TX (msb) movff spiReg, id3 call spi_read ;pulse duration 1 movff spiReg, ip1l call spi_read movff spiReg, ip1h call spi_read ;pulse duration 2 movff spiReg, ip2l call spi_read movff spiReg, ip2h call spi_read ;pulse duration 3 movff spiReg, ip3l call spi_read movff spiReg, ip3h call spi_read ;pulse duration 4 movff spiReg, ip4l call spi_read movff spiReg, ip4h call spi_read ;pulse duration 5 movff spiReg, ip5l call spi_read movff spiReg, ip5h call spi_read ;pulse duration 6 movff spiReg, ip6l call spi_read movff spiReg, ip6h call spi_read ;pulse duration 7 movff spiReg, ip7l call spi_read movff spiReg, ip7h call spi_read ;pulse duration 8 movff spiReg, ip8l call spi_read movff spiReg, ip8h SPI_OFF return ;************** ; init_A7105 ;************** init_A7105 ;reset A7105 spi_write_ladr_lval 0x00, 0x00 ;ID A7105 <- 0x5475c52a ??? useful ??? SPI_ON movlwf 0x06, spiReg call spi_write movlwf 0x54, spiReg call spi_write movlwf 0x75, spiReg call spi_write movlwf 0xc5, spiReg call spi_write movlwf 0x2A, spiReg call spi_write SPI_OFF ;read ID A7105 -> id0..id3 to check spi dialog SPI_ON movlwf 0x46, spiReg call spi_write call spi_read movff spiReg, id0 call spi_read movff spiReg, id1 call spi_read movff spiReg, id2 call spi_read movff spiReg, id3 SPI_OFF ;init A7105 regs clrf tableAdress ;reset table adress movlwf 0xFF, adressReg ;init reg adress = -1 movlwf 0x33, cptReg ;init loop counter BoucleInitReg incf adressReg, f ;increment reg adress movf adressReg, w ;init w pour "brw" (relative branch with w) call tableInitReg ;read byte in tableInitReg -> valReg movwf valReg incf tableAdress, f ;increment table adress incf valReg, w ;test valReg == 0xFF ? bz BoucleInitReg ;yes -> skip reg init spi_write_fadr_fval adressReg, valReg decfsz cptReg, f ;test loop end ? goto BoucleInitReg ;no -> loop ;IF Filter Bank Calibration spi_write_ladr_lval 0x02, 0x01 waitCalibrationFilter ;wait calibration end spi_read_ladr 0x02 btfsc spiReg, 0 goto waitCalibrationFilter ;test calibration ok ? ;VCO Current Calibration spi_write_ladr_lval 0x24, 0x013 ;VCO Bank Calibration spi_write_ladr_lval 0x26, 0x03B ;VCO Bank Calibrate channel 0x00? spi_write_ladr_lval 0x0F, 0x00 ;Set Channel 0 spi_write_ladr_lval 0x02, 0x02 ;VCO Calibration waitCalibrationVCO ;wait calibration end spi_read_ladr 0x02 btfsc spiReg, 1 goto waitCalibrationVCO ;test calibration ok ? ;VCO Bank Calibrate channel 0xa0? spi_write_ladr_lval 0x0F, 0xA0 ;Set Channel 0xa0 spi_write_ladr_lval 0x02, 0x02 ;VCO Calibration waitCalibrationVCObis ;wait calibration end spi_read_ladr 0x02 btfsc spiReg, 1 goto waitCalibrationVCObis ;test calibration ok ? ;Reset VCO Band calibration spi_write_ladr_lval 0x25, 0x08 return ;************** ; bind_TX ;************* bind_TX ;wait for valid bind packet for about 2,5s on channel 0 ;(256 test loop max, 10ms each loop) clrf cptTimeOut ;init time out counter waitValidBindPacket spi_write_strobe_command 0xA0 ;A0 -> standby mode spi_write_strobe_command 0xF0 ;F0 -> reset RX FIFO spi_write_ladr_lval 0x0F, 0x00 ;Set Channel 0 spi_write_strobe_command 0xC0 ;C0 -> get into RX mode waitBindPacketOrTimeOut startTimer4Nus .10000 ;wait for 10ms wait10ms banksel PIR3 btfss PIR3, TMR4IF goto wait10ms banksel PORTA btfsc cptTimeOut, 2 ;12Hz blinking LED while binding bcf LED ;LED OFF btfss cptTimeOut, 2 bsf LED ;LED ON btfss GIO ;received RX packet ? goto testValidBindPacket ;yes -> test valid packet ? decfsz cptTimeOut, f ;decremente time out counter, roll-off? goto waitBindPacketOrTimeOut ;no -> wait again goto timeOutBindPacket ;yes -> time out testValidBindPacket spi_read_ladr 0x00 ;read mode register btfsc spiReg, 5 ;test CRC (bit5) goto waitValidBindPacket ;not valid, wait again btfsc spiReg, 6 ;test CEF (bit6) goto waitValidBindPacket ;not valid, wait again ;a valid bind packet was received -> write 4 bytes ID ;in PIC EEPROM to avoid binding each time RX is switched on call read_packet ;read packet which contains TX ID clrf adr_pic movff id0, data_pic ;write id0, eeprom adress 0x00 call write_eeprom_pic incf adr_pic, f movff id1, data_pic ;write id1, eeprom adress 0x01 call write_eeprom_pic incf adr_pic, f movff id2, data_pic ;write id2, eeprom adress 0x02 call write_eeprom_pic incf adr_pic, f movff id3, data_pic ;write id3, eeprom adress 0x03 call write_eeprom_pic movff id0, txid0 ;tx id = current id movff id1, txid1 movff id2, txid2 movff id3, txid3 return timeOutBindPacket ;could not bind to TX => read previous TX ID stored in PIC EEPROM clrf adr_pic ;read id0 @ 0x00 call read_eeprom_pic movff data_pic, txid0 incf adr_pic, f ;read id1 @ 0x01 call read_eeprom_pic movff data_pic, txid1 incf adr_pic, f ;read id2 @ 0x02 call read_eeprom_pic movff data_pic, txid2 incf adr_pic, f ;read id3 @ 0x03 call read_eeprom_pic movff data_pic, txid3 return ;*********************** ; readNextChannelNumber ;*********************** readNextChannelNumber decfsz cptChannel, f ;decrement channel counter goto tableReadChannel ;cptChannel > 0 => skip sequence reset resetChannelSequence movlwf .16, cptChannel ;cptChannel = 16 movff tableOffset, tableAdress ;load adress of tableChannels tableReadChannel movf tableAdress, w call tableChannels ;read channel number in table movwf channel decf channel, f ;dec channel number movf channelOffset, w ;substract channel offset from channel number subwf channel, f incf tableAdress, f ;incremente table adress return ;------------------------------------------------------------------------------ ; MAIN PROGRAM ;------------------------------------------------------------------------------ START ;------------------------------------------------------------------------------ ; PLACE USER PROGRAM HERE ;------------------------------------------------------------------------------ bcf INTCON, GIE ; disable global interrupt ; set internal oscillator speed Fosc=16Mhz ; => internal instruction cycle speed = Fosc/4 = 4Mhz banksel OSCCON bsf OSCCON, IRCF3 bsf OSCCON, IRCF2 bsf OSCCON, IRCF1 bsf OSCCON, IRCF0 ;disable analog inputs to enable digital inputs banksel ANSELA clrf ANSELA clrf ANSELC ;config TRIS A,B,C banksel TRISC bcf LED_TRIS ;LED digital output bcf CS_SPI_TRIS ;CS digital output bcf SDIO_SPI_TRIS ;SDIO digital output bcf SCK_SPI_TRIS ;SCK digital output bcf SERVO1_TRIS ;SERVO1 digital output bcf SERVO2_TRIS ;SERVO2 digital output bcf SERVO3_TRIS ;SERVO3 digital output bcf SERVO4_TRIS ;SERVO4 digital output bcf SERVO5_TRIS ;SERVO5 digital output bcf SERVO6_TRIS ;SERVO6 digital output ;config PORT A,B,C banksel PORTC clrf PORTA clrf PORTC bsf LED ;LED ON bsf CS_SPI ;CS default HIGH bsf SDIO_SPI ;SDIO default HIGH bcf SCK_SPI ;SCK default LOW ;init mask for servo output on portb and portc ;servo0,6,7 does not exist but it simplifies algorythm... movlwf 0x00, maskServo0Porta ; movlwf 0x00, maskServo0Portc ; movlwf 0x01, maskServo1Porta ;servo 1 -> porta0 movlwf 0x00, maskServo1Portc ; movlwf 0x00, maskServo2Porta ; movlwf 0x02, maskServo2Portc ;servo 2 -> portc1 movlwf 0x00, maskServo3Porta ; movlwf 0x04, maskServo3Portc ;servo 3 -> portc2 movlwf 0x00, maskServo4Porta ; movlwf 0x01, maskServo4Portc ;servo 4 -> portc0 movlwf 0x02, maskServo5Porta ;servo 5 -> porta1 movlwf 0x00, maskServo5Portc ; movlwf 0x00, maskServo6Porta ; #ifdef PPM movlwf 0x00, maskServo6Portc ; #else movlwf 0x10, maskServo6Portc ;servo 6 -> portc4 #endif ;set timer1 ( counts servos pulse duration, 1us step ) ;16 bit timer banksel T1CON clrf T1CON ; reset timer1 control register clrf T1GCON ; reset timer1 gate control register bsf T1CON, T1CKPS1 ; prescaler 1:4 (1us period) bcf PIR1, TMR1IF ; clear flag interrupt timer1 banksel PIE1 bsf PIE1, TMR1IE ; enable interrupt timer1 ;set timer2 ( starts servos pulse sequence every 20ms, 100hz interrupt ) ;8 bits timer with comparator register banksel PR2 movlwf .249, PR2 ; Timer2 comparator register => 4Mhz/16/(249+1)/10=100hz banksel T2CON clrf T2CON ; reset timer2 control register bsf T2CON, T2OUTPS3 ; 1:10 postscaler bsf T2CON, T2OUTPS0 ; bsf T2CON, T2CKPS1 ; 1:16 prescaler banksel PIR1 bcf PIR1, TMR2IF ; clear flag interrupt timer2 banksel PIE1 bsf PIE1, TMR2IE ; enable interrupt timer2 ;set timer4 ( counts time-out delay, 40us resolution ) ;8 bits timer with comparator register banksel T4CON clrf T4CON ; reset timer4 control register bsf T4CON, T4OUTPS3 ; 1:10 postscaler bsf T4CON, T4OUTPS0 ; bsf T4CON, T4CKPS1 ; 1:16 prescaler banksel PIR3 bcf PIR3, TMR4IF ; clear flag interrupt timer4 #ifdef PPM ;set timer6 ( counts 400us PPM pulse ) ;8 bits timer with comparator register banksel T6CON clrf T6CON ; reset timer6 control register, 1:1 postscaler bsf T6CON, T6CKPS1 ; 1:16 prescaler movlwf .100, PR6 ; load period register (=400us @ 250khz = Fc/4/16) banksel PIR3 bcf PIR3, TMR6IF ; clear flag interrupt timer6 banksel PIE3 bsf PIE3, TMR6IE ; enable interrupt timer6 #endif ;enable interrupts from timer 1 and 2 banksel INTCON bsf INTCON, PEIE ; enable peripheral interrupt bsf INTCON, GIE ; enable global interrupt ;init servo position middle movlw low .1500 ;1500us (low byte) movwf op1l movwf op2l movwf op3l movwf op4l movwf op5l movwf op6l movlw high .1500 ;1500us (high byte) movwf op1h movwf op2h movwf op3h movwf op4h movwf op5h movwf op6h banksel T2CON bsf T2CON, TMR2ON ;timer2 on => starts servo pulse output !!! ;wait 10ms to be sure A7105 wake up startTimer4Nus .10000 ;start timer4 for 10ms pause10ms banksel PIR3 btfss PIR3, TMR4IF goto pause10ms ;init A7105 call init_A7105 nop ;bind with T call bind_TX ;------------------------------------------------------------------ ;now we have TX ID (previous TX or just binded TX) ;we read each packet in a frequency sequence which depends on TX id ;(frequency sequences are in tableChannels) swapf txid0, w ;init table read channel sequence andlw 0xF0 ;compute tableOffset <- (id0 & 0x0F)*16 movwf tableOffset movlwf 0x01, cptChannel ;init channel counter (goes from 16 to 1) swapf txid0, w ;compute channel offset andlw 0x0F ;channelOffset <- (id0 / 16) movwf channelOffset movlw .10 ;if channelOffset >= 10 then channelOffset <- 9 subwf channelOffset, w movlw .9 btfsc STATUS, C ;(C=1 if W<=F) movwf channelOffset ;*********** ; MAIN LOOP ;*********** scanChannels call readNextChannelNumber ;read next channel number in tableChannels spi_write_strobe_command 0xA0 ;A0 -> standby mode spi_write_strobe_command 0xF0 ;F0 -> reset RX FIFO spi_write_ladr_fval 0x0F, channel ;set channel spi_write_strobe_command 0xC0 ;C0 -> get into RX mode startTimer4Nus .2000 ;start timer4 for 2ms time out ;bsf LED ;for debug... waitRxPacket banksel PIR3 btfsc PIR3, TMR4IF ;test time-out 2ms ? goto timeOutRxPacket ; banksel PORTA btfsc GIO ;test packet available ? goto waitRxPacket ;no -> wait spi_read_ladr 0x00 ;read "mode register" btfsc spiReg, 5 ;test CRC (bit5) goto waitRxPacket ;not valid, wait again btfsc spiReg, 6 ;test CEF (bit6) goto waitRxPacket ;not valid, wait again call read_packet ;read RX packet movf txid0, w ;good TX ID0 ? cpfseq id0 goto waitRxPacket movf txid1, w ;good TX ID1 ? cpfseq id1 goto waitRxPacket movf txid2, w ;good TX ID2 ? cpfseq id2 goto waitRxPacket movf txid3, w ;good TX ID3 ? cpfseq id3 goto waitRxPacket banksel PORTA bsf LED ;led ON = rx ok ;force pulse duration to 10us to check accuracy ;movlw .10 ;movwf ip1l ;movwf ip2l ;movwf ip3l ;movwf ip4l ;movwf ip5l ;movwf ip6l ;clrf ip1h ;clrf ip2h ;clrf ip3h ;clrf ip4h ;clrf ip5h ;clrf ip6h banksel INTCON bcf INTCON, GIE ; disable global interrupt movff ip1h, op1h movff ip1l, op1l bsf INTCON, GIE ; enable global interrupt nop nop bcf INTCON, GIE ; disable global interrupt movff ip2h, op2h movff ip2l, op2l bsf INTCON, GIE ; enable global interrupt nop nop bcf INTCON, GIE ; disable global interrupt movff ip3h, op3h movff ip3l, op3l bsf INTCON, GIE ; enable global interrupt nop nop bcf INTCON, GIE ; disable global interrupt movff ip4h, op4h movff ip4l, op4l bsf INTCON, GIE ; enable global interrupt nop nop bcf INTCON, GIE ; disable global interrupt movff ip5h, op5h movff ip5l, op5l bsf INTCON, GIE ; enable global interrupt nop nop bcf INTCON, GIE ; disable global interrupt movff ip6h, op6h movff ip6l, op6l bsf INTCON, GIE ; enable global interrupt goto scanChannels timeOutRxPacket banksel PORTA bcf LED ;switch off LED call readNextChannelNumber ;skip next channel because we probably allready missed it goto scanChannels ;scan next channel ;********************************************************** ;*********************** TABLES *************************** ;********************************************************** ;A7105 config registers init value tableInitReg brw retlw 0xFF retlw 0x42 retlw 0x00 retlw 0x14 retlw 0x00 retlw 0xFF retlw 0xFF retlw 0x00 retlw 0x00 retlw 0x00 retlw 0x00 retlw 0x01 retlw 0x21 retlw 0x05 retlw 0x00 retlw 0x50 retlw 0x9e retlw 0x4b retlw 0x00 retlw 0x02 retlw 0x16 retlw 0x2b retlw 0x12 retlw 0x00 retlw 0x62 retlw 0x80 retlw 0x80 retlw 0x00 retlw 0x0a retlw 0x32 retlw 0xc3 retlw 0x0f retlw 0x13 retlw 0xc3 retlw 0x00 retlw 0xFF retlw 0x00 retlw 0x00 retlw 0x3b retlw 0x00 retlw 0x17 retlw 0x47 retlw 0x80 retlw 0x03 retlw 0x01 retlw 0x45 retlw 0x18 retlw 0x00 retlw 0x01 retlw 0x0f retlw 0xFF ;substract one from each tableChannels value to have good channels numbers ... tableChannels brw ;line 0 retlw 0x0a retlw 0x5a retlw 0x14 retlw 0x64 retlw 0x1e retlw 0x6e retlw 0x28 retlw 0x78 retlw 0x32 retlw 0x82 retlw 0x3c retlw 0x8c retlw 0x46 retlw 0x96 retlw 0x50 retlw 0xa0 ;line 1 retlw 0xa0 retlw 0x50 retlw 0x96 retlw 0x46 retlw 0x8c retlw 0x3c retlw 0x82 retlw 0x32 retlw 0x78 retlw 0x28 retlw 0x6e retlw 0x1e retlw 0x64 retlw 0x14 retlw 0x5a retlw 0x0a ;line 2 retlw 0x0a retlw 0x5a retlw 0x50 retlw 0xa0 retlw 0x14 retlw 0x64 retlw 0x46 retlw 0x96 retlw 0x1e retlw 0x6e retlw 0x3c retlw 0x8c retlw 0x28 retlw 0x78 retlw 0x32 retlw 0x82 ;line 3 retlw 0x82 retlw 0x32 retlw 0x78 retlw 0x28 retlw 0x8c retlw 0x3c retlw 0x6e retlw 0x1e retlw 0x96 retlw 0x46 retlw 0x64 retlw 0x14 retlw 0xa0 retlw 0x50 retlw 0x5a retlw 0x0a ;line 4 retlw 0x28 retlw 0x78 retlw 0x0a retlw 0x5a retlw 0x50 retlw 0xa0 retlw 0x14 retlw 0x64 retlw 0x1e retlw 0x6e retlw 0x3c retlw 0x8c retlw 0x32 retlw 0x82 retlw 0x46 retlw 0x96 ;line 5 retlw 0x96 retlw 0x46 retlw 0x82 retlw 0x32 retlw 0x8c retlw 0x3c retlw 0x6e retlw 0x1e retlw 0x64 retlw 0x14 retlw 0xa0 retlw 0x50 retlw 0x5a retlw 0x0a retlw 0x78 retlw 0x28 ;line 6 retlw 0x50 retlw 0xa0 retlw 0x28 retlw 0x78 retlw 0x0a retlw 0x5a retlw 0x1e retlw 0x6e retlw 0x3c retlw 0x8c retlw 0x32 retlw 0x82 retlw 0x46 retlw 0x96 retlw 0x14 retlw 0x64 ;line 7 retlw 0x64 retlw 0x14 retlw 0x96 retlw 0x46 retlw 0x82 retlw 0x32 retlw 0x8c retlw 0x3c retlw 0x6e retlw 0x1e retlw 0x5a retlw 0x0a retlw 0x78 retlw 0x28 retlw 0xa0 retlw 0x50 ;line 8 retlw 0x50 retlw 0xa0 retlw 0x46 retlw 0x96 retlw 0x3c retlw 0x8c retlw 0x28 retlw 0x78 retlw 0x0a retlw 0x5a retlw 0x32 retlw 0x82 retlw 0x1e retlw 0x6e retlw 0x14 retlw 0x64 ;line 9 retlw 0x64 retlw 0x14 retlw 0x6e retlw 0x1e retlw 0x82 retlw 0x32 retlw 0x5a retlw 0x0a retlw 0x78 retlw 0x28 retlw 0x8c retlw 0x3c retlw 0x96 retlw 0x46 retlw 0xa0 retlw 0x50 ;line 10 retlw 0x46 retlw 0x96 retlw 0x3c retlw 0x8c retlw 0x50 retlw 0xa0 retlw 0x28 retlw 0x78 retlw 0x0a retlw 0x5a retlw 0x1e retlw 0x6e retlw 0x32 retlw 0x82 retlw 0x14 retlw 0x64 ;line 11 retlw 0x64 retlw 0x14 retlw 0x82 retlw 0x32 retlw 0x6e retlw 0x1e retlw 0x5a retlw 0x0a retlw 0x78 retlw 0x28 retlw 0xa0 retlw 0x50 retlw 0x8c retlw 0x3c retlw 0x96 retlw 0x46 ;line 12 retlw 0x46 retlw 0x96 retlw 0x0a retlw 0x5a retlw 0x3c retlw 0x8c retlw 0x14 retlw 0x64 retlw 0x50 retlw 0xa0 retlw 0x28 retlw 0x78 retlw 0x1e retlw 0x6e retlw 0x32 retlw 0x82 ;line 13 retlw 0x82 retlw 0x32 retlw 0x6e retlw 0x1e retlw 0x78 retlw 0x28 retlw 0xa0 retlw 0x50 retlw 0x64 retlw 0x14 retlw 0x8c retlw 0x3c retlw 0x5a retlw 0x0a retlw 0x96 retlw 0x46 ;line 14 retlw 0x46 retlw 0x96 retlw 0x0a retlw 0x5a retlw 0x50 retlw 0xa0 retlw 0x3c retlw 0x8c retlw 0x28 retlw 0x78 retlw 0x1e retlw 0x6e retlw 0x32 retlw 0x82 retlw 0x14 retlw 0x64 ;line 15 retlw 0x64 retlw 0x14 retlw 0x82 retlw 0x32 retlw 0x6e retlw 0x1e retlw 0x78 retlw 0x28 retlw 0x8c retlw 0x3c retlw 0xa0 retlw 0x50 retlw 0x5a retlw 0x0a retlw 0x96 retlw 0x46 ;*********************** F I N ********************** END