;************************************************************************* ; RX A7105 2.4Ghz protocol Flysky/turnigy 9x (v2) ; look at thread: http://www.rcgroups.com/forums/showthread.php?t=1626362 ; many thanks to: PhracturedBlue, Dave1993, Surfbreizh, CristiV... ; autor: Thierry Pébayle ;************************************************************************* ; include PIC reg definitions. include PROCESSOR PIC18F14K22 ;configuration bits ; ;Background Debug: CONFIG DEBUG = ON ;Enabled ; CONFIG DEBUG = OFF ;Disabled ;Oscillator Selection bits: ; CONFIG FOSC = LP ; LP oscillator ; CONFIG FOSC = XT ; XT oscillator ; CONFIG FOSC = HS ; HS oscillator ; CONFIG FOSC = ERCCLKOUT ; External RC oscillator, CLKOUT function on OSC2 ; CONFIG FOSC = ECCLKOUTH ; EC, CLKOUT function on OSC2 (high) ; CONFIG FOSC = ECH ; EC (high) ; CONFIG FOSC = ERC ; External RC oscillator CONFIG FOSC = IRC ; Internal RC oscillator ; CONFIG FOSC = IRCCLKOUT ; Internal RC oscillator, CLKOUT function on OSC2 ; CONFIG FOSC = ECCLKOUTM ; EC, CLKOUT function on OSC2 (medium) ; CONFIG FOSC = ECM ; EC (medium) ; CONFIG FOSC = ECCLKOUTL ; EC, CLKOUT function on OSC2 (low) ; CONFIG FOSC = ECL ; EC (low) ; 4 X PLL Enable bit: CONFIG PLLEN = OFF ; PLL is under software control ; CONFIG PLLEN = ON ; Oscillator multiplied by 4 ; Primary Clock Enable Bit: CONFIG PCLKEN = OFF ; Primary clock is under software control ; CONFIG PCLKEN = ON ; Primary clock enabled ; Fail-Safe Clock Monitor Enable bit: CONFIG FCMEN = OFF ; Fail-Safe Clock Monitor disabled ; CONFIG FCMEN = ON ; Fail-Safe Clock Monitor enabled ; Internal/External Oscillator Switchover bit: CONFIG IESO = OFF ; Oscillator Switchover mode disabled ; CONFIG IESO = ON ; Oscillator Switchover mode enabled ; Power-up Timer Enable bit: ; CONFIG PWRTEN = ON ; PWRT enabled CONFIG PWRTEN = OFF ; PWRT disabled ; Brown-out Reset Enable bits: CONFIG BOREN = OFF ; Brown-out Reset disabled in hardware and software ; CONFIG BOREN = ON ; Brown-out Reset enabled and controlled by software (SBOREN is enabled) ; CONFIG BOREN = NOSLP ; Brown-out Reset enabled in hardware only and disabled in Sleep mode (SBOREN is disabled) ; CONFIG BOREN = SBORDIS ; Brown-out Reset enabled in hardware only (SBOREN is disabled) ; Brown Out Voltage: ; CONFIG BORV = 30 ; VBOR set to 2.85 V nominal CONFIG BORV = 27 ; VBOR set to 2.5 V nominal ; CONFIG BORV = 22 ; VBOR set to 2.2 V nominal ; CONFIG BORV = 19 ; VBOR set to 1.9 V nominal ; Watchdog Timer Enable bit: CONFIG WDTEN = OFF ; WDT is controlled by SWDTEN bit of the WDTCON reg ; CONFIG WDTEN = ON ; WDT is always enabled. SWDTEN bit has no effect. ; Watchdog Timer Postscale Select bits: ; CONFIG WDTPS = 1 ; 1:1 ; CONFIG WDTPS = 2 ; 1:2 ; CONFIG WDTPS = 4 ; 1:4 ; CONFIG WDTPS = 8 ; 1:8 ; CONFIG WDTPS = 16 ; 1:16 ; CONFIG WDTPS = 32 ; 1:32 ; CONFIG WDTPS = 64 ; 1:64 ; CONFIG WDTPS = 128 ; 1:128 ; CONFIG WDTPS = 256 ; 1:256 ; CONFIG WDTPS = 512 ; 1:512 CONFIG WDTPS = 1024 ; 1:1024 x 4 ms = about 4 seconds ; CONFIG WDTPS = 2048 ; 1:2048 ; CONFIG WDTPS = 4096 ; 1:4096 ; CONFIG WDTPS = 8192 ; 1:8192 ; CONFIG WDTPS = 16384 ; 1:16384 ; CONFIG WDTPS = 32768 ; 1:32768 ; MCLR Pin Enable bit: ; CONFIG MCLRE = OFF ; RA3 input pin enabled CONFIG MCLR disabled CONFIG MCLRE = ON ; MCLR pin enabled, RA3 input pin disabled ; HFINTOSC Fast Start-up bit: CONFIG HFOFST = OFF ; The system clock is held off until the HFINTOSC is stable. ; CONFIG HFOFST = ON ; HFINTOSC starts clocking the CPU without waiting for the oscillator to stablize. ; Stack Full/Underflow Reset Enable bit: CONFIG STVREN = OFF ; Stack full/underflow will not cause Reset ; CONFIG STVREN = ON ; Stack full/underflow will cause Reset ; Single-Supply ICSP Enable bit: CONFIG LVP = OFF ; Single-Supply ICSP disabled ; CONFIG LVP = ON ; Single-Supply ICSP enabled ; Boot Block Size Select Bit: CONFIG BBSIZ = OFF ; 1kW boot block size ; CONFIG BBSIZ = ON ; 2kW boot block size ; Extended Instruction Set Enable bit: CONFIG XINST = OFF ; Instruction set extension and Indexed Addressing mode disabled (Legacy mode) ; CONFIG XINST = ON ; Instruction set extension and Indexed Addressing mode enabled ; Code Protection bit: ; CONFIG CP0 = ON ; Block 0 code-protected CONFIG CP0 = OFF ; Block 0 not code-protected ; Code Protection bit: ; CONFIG CP1 = ON ; Block 1 code-protected CONFIG CP1 = OFF ; Block 1 not code-protected ; Boot Block Code Protection bit: ; CONFIG CPB = ON ; Boot block code-protected CONFIG CPB = OFF ; Boot block not code-protected ; Data EEPROM Code Protection bit: ; CONFIG CPD = ON ; Data EEPROM code-protected CONFIG CPD = OFF ; Data EEPROM not code-protected ; Write Protection bit: ; CONFIG WRT0 = ON ; Block 0 write-protected CONFIG WRT0 = OFF ; Block 0 not write-protected ; Write Protection bit: ; CONFIG WRT1 = ON ; Block 1 write-protected CONFIG WRT1 = OFF ; Block 1 not write-protected ; Boot Block Write Protection bit: ; CONFIG WRTB = ON ; Boot block write-protected CONFIG WRTB = OFF ; Boot block not write-protected ; Configuration Register Write Protection bit: ; CONFIG WRTC = ON ; Configuration regs write-protected CONFIG WRTC = OFF ; Configuration regs not write-protected ; Data EEPROM Write Protection bit: ; CONFIG WRTD = ON ; Data EEPROM write-protected CONFIG WRTD = OFF ; Data EEPROM not write-protected ; Table Read Protection bit: ; CONFIG EBTR0 = ON ; Block 0 protected from table reads executed in other blocks CONFIG EBTR0 = OFF ; Block 0 not protected from table reads executed in other blocks ; Table Read Protection bit: ; CONFIG EBTR1 = ON ; Block 1 protected from table reads executed in other blocks CONFIG EBTR1 = OFF ; Block 1 not protected from table reads executed in other blocks ; Boot Block Table Read Protection bit: ; CONFIG EBTRB = ON ; Boot block protected from table reads executed in other blocks CONFIG EBTRB = OFF ; Boot block not protected from table reads executed in other blocks ;****************************************************************************** ; MACRO ;****************************************************************************** ;move literal into register through W ;literal -> W -> register movlwf MACRO valeur, reg movlw valeur movwf reg ENDM ;start timer3 for N us count before overflow startTimer3Nus MACRO duration bcf T3CON, TMR3ON ;stops timer3 movlw high duration movwf TMR3H comf TMR3H, f ;complement high byte movlw low duration movwf TMR3L comf TMR3L, f ;complement low byte bcf PIR2, TMR3IF ;clear timer3 flag bsf T3CON, TMR3ON ;starts timer3 ENDM ;init table read adress initTableRead MACRO adrTable movlw high adrTable movwf TBLPTRH movlw low adrTable movwf TBLPTRL ENDM ;one clock pulse for spi com SCK_SPI_PULSE MACRO ;one clock (rising edge then falling edge) 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 ;****************************************************************************** ; CONSTANTES ;****************************************************************************** ;LED #define LED PORTC, 2 #define LED_TRIS TRISC, 2 ;com spi A7105 #define CS_SPI PORTB,7 ;chip select SPI #define SCK_SPI PORTB,6 ;serial clock SPI #define SDIO_SPI PORTC,7 ;serial data input/output SPI #define GIO PORTB,4 ;GIO output A7105 #define CS_SPI_TRIS TRISB,7 ;chip select SPI #define SCK_SPI_TRIS TRISB,6 ;serial clock SPI #define SDIO_SPI_TRIS TRISC,7 ;serial data input/output SPI #define GIO_TRIS TRISB,4 ;GIO output A7105 #define SPI_ON bcf CS_SPI ;enable SPI A7105 #define SPI_OFF bsf CS_SPI ;desable SPI A7105 ;servo outputs #define SERVO1 PORTB, 5 #define SERVO2 PORTC, 1 #define SERVO3 PORTC, 5 #define SERVO4 PORTC, 6 #define SERVO5 PORTC, 3 #define SERVO6 PORTC, 4 #define SERVO1_TRIS TRISB, 5 #define SERVO2_TRIS TRISC, 1 #define SERVO3_TRIS TRISC, 5 #define SERVO4_TRIS TRISC, 6 #define SERVO5_TRIS TRISC, 3 #define SERVO6_TRIS TRISC, 4 ;****************************************************************************** ; VARIABLES ;****************************************************************************** CBLOCK 0x60 ; Sample GPR variable reg allocations ;init A7105 cptReg ;spi_write, spi_read cptBit, spiReg, adressReg ;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 8 (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 ;mask to set or unset servo output on portb & portc ;servo0 and servo7 does not exist but it simplifies algorythm... maskServo0Portb, maskServo0Portc maskServo1Portb, maskServo1Portc maskServo2Portb, maskServo2Portc maskServo3Portb, maskServo3Portc maskServo4Portb, maskServo4Portc maskServo5Portb, maskServo5Portc maskServo6Portb, maskServo6Portc maskServo7Portb, maskServo7Portc cptItTimer0 ;timer0 it counter cptItTimer2 ;timer2 it counter ENDC W_TEMP EQU 0x000 ; w reg for context saving (ACCESS) STATUS_TEMP EQU 0x001 ; status used for context saving BSR_TEMP EQU 0x002 ; bank select used for ISR context saving ;****************************************************************************** ; EEPROM INITIALIZATION ; ; The 18F14K22 has 256 bytes of non-volatile EEPROM starting at 0xF00000 ; ;****************************************************************************** DATAEE ORG 0xF00000 ; Starting address for EEPROM for 18F14K22 DE "MCHP" ; Place 'M' 'C' 'H' 'P' at address 0,1,2,3 ;****************************************************************************** ; RESET VECTOR ;****************************************************************************** RES_VECT ORG 0x0000 ; processor reset vector GOTO START ; go to beginning of program ;****************************************************************************** ; HIGH PRIORITY INTERRUPT VECTOR ;****************************************************************************** ISRH ORG 0x0008 ; Run the High Priority Interrupt Service Routine GOTO HIGH_ISR ;****************************************************************************** ; LOW PRIORITY INTERRUPT VECTOR ;****************************************************************************** ISRL ORG 0x0018 ; Run the High Priority Interrupt Service Routine GOTO LOW_ISR ;****************************************************************************** ; HIGH PRIORITY INTERRUPT SERVICE ROUTINE ;****************************************************************************** HIGH_ISR ; Insert High Priority ISR Here ; in "compatibility mode" (RCON,IPEN = 0) all interrupts goes here !!! ; timer2 starts pulse sequence(50Hz), then timer0 counts pulse duration for ; each servo ;test timer2 interrupt or timer0 interrupt ? btfss PIR1, TMR2IF goto timer0_isr timer2_isr ;(100hz) bcf PIR1, TMR2IF ;clear timer2 it flag incf cptItTimer2, f ;incremente timer2 it counter btfsc cptItTimer2, 0 ;skip if odd RETFIE FAST ;quit interrupt, automatic context saving (W, STATUS, BSR) ;start_pulse_sequence (50hz) LFSR 0, op1h ;load pulse duration 1 into indirect register FSR0 LFSR 1, maskServo0Portb ;load mask servo0 into indirect register FSR1 movlwf .7, cptItTimer0 ;init timer0 it counter movlwf 0xFF, TMR0H ;timer0 = 0xFFFF => timer0 overflow immediatly movlwf 0xFF, TMR0L ; bsf T0CON, TMR0ON ;timer0 on RETFIE FAST ;quit interrupt, automatic context saving (W, STATUS, BSR) timer0_isr comf POSTINC1, w ;stop previous pulse andwf PORTB comf POSTINC1, w andwf PORTC bcf T0CON, TMR0ON ;timer0 off bcf INTCON, TMR0IF ;clear timer0 it flag decf cptItTimer0, f ;decremente timer0 it counter comf POSTINC0, w ;set timer0 duration movwf TMR0H comf POSTINC0, w movwf TMR0L movf cptItTimer0, f ;test fin sequence, cptItTimer0 = 0 ? btfss STATUS, Z bsf T0CON, TMR0ON ;no -> timer0 on nop ;wait 3us more to adjust timing nop nop nop nop nop nop nop nop nop nop nop movf POSTINC1, w ;start next pulse iorwf PORTB movf POSTDEC1, w iorwf PORTC RETFIE FAST ;quit interrupt, automatic context saving (W, STATUS, BSR) ;****************************************************************************** ; LOW PRIORITY INTERRUPT SERVICE ROUTINE ;****************************************************************************** LOW_ISR ; Context Saving for Low ISR ;MOVWF W_TEMP ; save W reg ;MOVFF STATUS, STATUS_TEMP ; save status reg ;MOVFF BSR, BSR_TEMP ; save bankselect reg ; Insert Low Priority ISR Here ; Context Saving for Low ISR ;MOVFF BSR_TEMP, BSR ; restore bankselect reg ;MOVF W_TEMP, W ; restore W reg ;MOVFF STATUS_TEMP, STATUS ; restore status reg RETFIE ;****************************************************** ;write_eeprom_pic ;****************************************************** write_eeprom_pic bcf EECON1, CFGS ;accès à EEPROM ou program bcf EECON1, EEPGD ;accès à EEPROM bcf PIR2, EEIF ;efface flag fin d'écriture movff data_pic, EEDATA movff adr_pic, 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 attenteFinEcriture btfss PIR2, EEIF goto attenteFinEcriture bcf EECON1, WREN ;désactive write return ;****************************************************** ;read_eeprom_pic ;****************************************************** read_eeprom_pic bcf EECON1, CFGS ;accès à EEPROM ou program bcf EECON1, EEPGD ;accès à EEPROM movff adr_pic, EEADR bsf EECON1, RD ;EE Read movff EEDATA, data_pic ;Move data to data_pic return ;**************************************** ; spi_write ; entrée: ; spiReg: valeur du registre à écrire ;**************************************** spi_write movlwf .8, cptBit boucleWriteReg btfss spiReg, 7 bcf SDIO_SPI btfsc spiReg, 7 bsf SDIO_SPI nop SCK_SPI_PULSE rlcf spiReg, f decfsz cptBit, f goto boucleWriteReg; return ;********************************* ; spi_read ; sortie: ; spiReg: valeur du registre lu ;********************************* spi_read bsf SDIO_SPI_TRIS ;SDIO input movlwf .8, cptBit clrf spiReg bcf STATUS, C boucleReadReg rlcf spiReg, f btfsc SDIO_SPI bsf spiReg, 0 SCK_SPI_PULSE decfsz cptBit, f goto boucleReadReg bcf SDIO_SPI_TRIS ;SDIO output 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 ;init A7105 regs initTableRead tableInitReg movlwf 0xFF, adressReg ;init reg adress -1 movlwf 0x33, cptReg ;init loop counter BoucleInitReg incf adressReg, f ;increment reg adress tblrd*+ ;read byte and increment table adress incf TABLAT, w ;test byte == 0xFF ? bz BoucleInitReg ;yes -> skip reg init spi_write_fadr_fval adressReg, TABLAT 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 startTimer3Nus .10000 ;wait for 10ms wait10ms btfss PIR2, TMR3IF goto wait10ms 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 ;read id1 @ 0x01 call read_eeprom_pic movff data_pic, txid1 incf adr_pic ;read id2 @ 0x02 call read_eeprom_pic movff data_pic, txid2 incf adr_pic ;read id3 @ 0x03 call read_eeprom_pic movff data_pic, txid3 return ;*********************** ; readNextChannelNumber ;*********************** readNextChannelNumber decfsz cptChannel ;decrement channel counter goto tableReadChannel ;cptChannel > 0 => skip sequence reset resetChannelSequence movlwf .16, cptChannel ;cptChannel = 16 initTableRead tableChannels ;load adress of tableChannels movf tableOffset, w addwf TBLPTRL, f ;add sequence offset in table btfsc STATUS, C incf TBLPTRH, f tableReadChannel tblrd*+ ;read channel number in table and increment table adress decf TABLAT, w ;dec channel number movwf channel return ;************** ;************** ; MAIN PROGRAM ;************** ;************** START ; set internal oscillator speed Fosc=16Mhz ; => internal instruction cycle speed = Fosc/4 = 4Mhz ; 111=16MHz, 110=8 MHz, 101=4MHz, 100=2MHz, 011= 1MHz(default after reset) ; 010=500kHz, 001=250kHz, 000=32kHz (LFINTOSC) bsf OSCCON, IRCF2 bsf OSCCON, IRCF1 bsf OSCCON, IRCF0 ;wait frequency is stable waitFreqStable btfss OSCCON, HFIOFS goto waitFreqStable ;config port A,B,C clrf PORTA clrf PORTB clrf PORTC bsf LED ;LED ON bcf LED_TRIS bsf CS_SPI ;CS default HIGH bcf CS_SPI_TRIS ;CS digital output bsf SDIO_SPI ;SDIO default HIGH bcf SDIO_SPI_TRIS ;SDIO digital output bcf SCK_SPI ;SCK default LOW 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 ;disable analog inputs to enable digital inputs clrf ANSEL clrf ANSELH ;init mask for servo output on portb and portc ;servo0 and servo7 does not exist but it simplifies algorythm... movlwf 0x00, maskServo0Portb ; movlwf 0x00, maskServo0Portc ; movlwf 0x20, maskServo1Portb ;portb5 movlwf 0x00, maskServo1Portc ; movlwf 0x00, maskServo2Portb ; movlwf 0x02, maskServo2Portc ;portc1 movlwf 0x00, maskServo3Portb ; movlwf 0x20, maskServo3Portc ;portc5 movlwf 0x00, maskServo4Portb ; movlwf 0x40, maskServo4Portc ;portc6 movlwf 0x00, maskServo5Portb ; movlwf 0x08, maskServo5Portc ;portc3 movlwf 0x00, maskServo6Portb ; movlwf 0x10, maskServo6Portc ;portc4 movlwf 0x00, maskServo7Portb ; movlwf 0x00, maskServo7Portc ; ;set timer0 ( counts servos pulse duration, 1us step ) ;16 bit timer clrf T0CON ; reset timer0 control register bsf T0CON, T0PS0 ; prescaler 1:4 (1us period) bcf INTCON, TMR0IF ; clear flag interrupt timer0 bsf INTCON, TMR0IE ; enable interrupt timer0 ;set timer2 ( starts servos pulse sequence every 20ms, 100hz interrupt ) ;8 bits timer with comparator register movlwf .249, PR2 ; Timer2 comparator register => 4Mhz/16/(249+1)/10=100hz clrf T2CON ; reset timer2 control register bsf T2CON, T2OUTPS3 ; 1:10 postscaler bsf T2CON, T2OUTPS0 ; bsf T2CON, T2CKPS1 ; 1:16 prescaler bcf PIR1, TMR2IF ; clear flag interrupt timer2 bsf PIE1, TMR2IE ; enable interrupt timer2 bsf IPR1, TMR2IP ; high priority bsf INTCON, PEIE ; enable low interrupt (usefull ?) bsf INTCON, GIE ; enable global interrupt ;set timer3 ( count time-out delay ) ;16 bit timer clrf T3CON ; reset timer3 control register bsf T3CON, RD16 ; set TMR3 bsf T3CON, T3CKPS1 ; 1:4 prescaler (1us period) bcf PIR2, TMR3IF ; clear flag interrupt timer3 ;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 bsf T2CON, TMR2ON ;timer2 on => starts servo pulse output !!! ;wait 10ms to be sure A7105 wake up startTimer3Nus .10000 ;start timer3 for 10ms pause10ms btfss PIR2, TMR3IF goto pause10ms ;init A7105 call init_A7105 nop ;bind with TX 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) ;************ ; 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 startTimer3Nus .1500 ;start timer3 for 1.5ms time out ;bsf LED ;for debug... waitRxPacket btfsc PIR2, TMR3IF ;test time-out 1.5ms ? goto timeOutRxPacket ; 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 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 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 bcf LED ;switch off LED call readNextChannelNumber ;skip next channel because we probably allready missed it goto scanChannels ;scan next channel ;********************************************************** ;*********************** TABLES *************************** ;********************************************************** tableInitReg db 0xFF, 0x42, 0x00, 0x14, 0x00, 0xFF, 0xFF ,0x00, 0x00, 0x00, 0x00, 0x01, 0x21, 0x05, 0x00, 0x50 db 0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x00, 0x62, 0x80, 0x80, 0x00, 0x0a, 0x32, 0xc3, 0x0f db 0x13, 0xc3, 0x00, 0xFF, 0x00, 0x00, 0x3b, 0x00, 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00 db 0x01, 0x0f, 0xFF ;substract one from each tableChannels value to have good channels numbers ... tableChannels db 0x0a, 0x5a, 0x14, 0x64, 0x1e, 0x6e, 0x28, 0x78, 0x32, 0x82, 0x3c, 0x8c, 0x46, 0x96, 0x50, 0xa0 db 0xa0, 0x50, 0x96, 0x46, 0x8c, 0x3c, 0x82, 0x32, 0x78, 0x28, 0x6e, 0x1e, 0x64, 0x14, 0x5a, 0x0a db 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x46, 0x96, 0x1e, 0x6e, 0x3c, 0x8c, 0x28, 0x78, 0x32, 0x82 db 0x82, 0x32, 0x78, 0x28, 0x8c, 0x3c, 0x6e, 0x1e, 0x96, 0x46, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a db 0x28, 0x78, 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96 db 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a, 0x78, 0x28 db 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96, 0x14, 0x64 db 0x64, 0x14, 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50 db 0x50, 0xa0, 0x46, 0x96, 0x3c, 0x8c, 0x28, 0x78, 0x0a, 0x5a, 0x32, 0x82, 0x1e, 0x6e, 0x14, 0x64 db 0x64, 0x14, 0x6e, 0x1e, 0x82, 0x32, 0x5a, 0x0a, 0x78, 0x28, 0x8c, 0x3c, 0x96, 0x46, 0xa0, 0x50 db 0x46, 0x96, 0x3c, 0x8c, 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64 db 0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50, 0x8c, 0x3c, 0x96, 0x46 db 0x46, 0x96, 0x0a, 0x5a, 0x3c, 0x8c, 0x14, 0x64, 0x50, 0xa0, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82 db 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0xa0, 0x50, 0x64, 0x14, 0x8c, 0x3c, 0x5a, 0x0a, 0x96, 0x46 db 0x46, 0x96, 0x0a, 0x5a, 0x50, 0xa0, 0x3c, 0x8c, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64 db 0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0x8c, 0x3c, 0xa0, 0x50, 0x5a, 0x0a, 0x96, 0x46 ;*********************** F I N ********************** END