;************************************************************************* ; mini octocopter ; autor: Thierry Pébayle ;************************************************************************* ; include PIC reg definitions. include PROCESSOR PIC18F14K22 ; 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 register ; 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, enough time to print teh help menu at this baudrate. ; 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 registers write-protected CONFIG WRTC = OFF ; Configuration registers 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 ;****************************************************************************** ; CONSTANTES ;****************************************************************************** ;LED #define LED PORTC, 6 #define LED_TRIS TRISC, 6 ;com spi A7105 et L3G4200 #define CS_RX LATC,5 ;chip select SPI RX A7105 #define CS_GYRO LATC,4 ;chip select SPI GYRO L3G4200 #define SCLK LATA,4 ;serial clock SPI #define SDIO PORTA,5 ;serial data input/output SPI #define GIO PORTA,2 ; A7105 GIO output #define CS_RX_TRIS TRISC,5 ;chip select SPI RX A7105 #define CS_GYRO_TRIS TRISC,4 ;chip select SPI GYRO L3G4200 #define SCLK_TRIS TRISA,4 ;serial clock SPI #define SDIO_TRIS TRISA,5 ;serial data input/output SPI #define GIO_TRIS TRISA,2 ;GIO output A7105 ;pwm outputs #define PWM1 LATB, 6 #define PWM2 LATB, 5 #define PWM3 LATB, 4 #define PWM4 LATC, 2 #define PWM5 LATC, 1 #define PWM6 LATC, 3 #define PWM7 LATB, 7 #define PWM8 LATC, 7 #define PWM1_TRIS TRISB, 6 #define PWM2_TRIS TRISB, 5 #define PWM3_TRIS TRISB, 4 #define PWM4_TRIS TRISC, 2 #define PWM5_TRIS TRISC, 1 #define PWM6_TRIS TRISC, 3 #define PWM7_TRIS TRISB, 7 #define PWM8_TRIS TRISC, 7 ;****************************************************************************** ; 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 ;duration -> TMR3 movwf TMR3H movlw low duration movwf TMR3L bcf STATUS, C ;TMR3 <- TMR3 * 2 rlcf TMR3L, f rlcf TMR3H, f comf TMR3H, f ;complement high byte 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 ;enable/disable CS for A7105 or L3G4200 ;RXorGYRO[0] = 1 => RX A7105 ;RXorGYRO[0] = 0 => GYRO L3G4200 SPI_ON MACRO btfsc RXorGYRO,0 bcf SCLK ;default state SCLK RX A7105 = 0 btfsc RXorGYRO,0 bsf CS_RX ;enable SPI RX A7105 btfss RXorGYRO,0 bsf SCLK ;default state SCLK GYRO L3G4200 = 1 btfss RXorGYRO,0 bsf CS_GYRO ;enable SPI GYRO L3G4200 ENDM SPI_OFF MACRO btfsc RXorGYRO,0 bcf CS_RX ;disable SPI A7105 btfss RXorGYRO,0 bcf CS_GYRO ;disable SPI L3G4200 ENDM ;one clock pulse for spi com SCLK_PULSE MACRO btfsc RXorGYRO,0 ;one clock (rising edge then falling edge for RX A7105) bsf SCLK btfsc RXorGYRO,0 bcf SCLK btfss RXorGYRO,0 ;one clock (falling edge then rising edge for GYRO L3G4200) bcf SCLK btfss RXorGYRO,0 bsf SCLK 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 ;update pwm counter and output update_pwm MACRO cpt, port, bit movf cpt, f btfsc STATUS, Z bcf port, bit btfss STATUS, Z decf cpt, f ENDM ;****************************************************************************** ; VARIABLES ;****************************************************************************** CBLOCK 0x60 ; Sample GPR variable reg allocations ;init A7105 cptReg ;spi_write, spi_read cptBit, spiReg, adressReg RXorGYRO ;bit0=1 => RX A7105, bit0=1 => GYRO L3G4200 ;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 ;pwm values and pwm counters valPWM1, valPWM2, valPWM3, valPWM4, valPWM5, valPWM6, valPWM7, valPWM8 cptPWM1, cptPWM2, cptPWM3, cptPWM4, cptPWM5, cptPWM6, cptPWM7, cptPWM8 cptChannel ;RX channel sequence counter (goes from 16 to 1) tableOffset ;offset sequence in tableSequence cptItTimer0 ;timer0 it counter cptItTimer2 ;timer2 it counter sync 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_isr ;(100khz) bcf LED bsf LED bcf PIR1, TMR2IF ;clear timer2 it flag decfsz cptItTimer2, f ;decremente timer2 it counter goto updatePWM ; cpt > 0 => updatePWM resetPWM ; cpt = 0 => resetPWM movff valPWM1, cptPWM1 movff valPWM2, cptPWM2 movff valPWM3, cptPWM3 movff valPWM4, cptPWM4 movff valPWM5, cptPWM5 movff valPWM6, cptPWM6 movff valPWM7, cptPWM7 movff valPWM8, cptPWM8 bsf PWM1 bsf PWM2 bsf PWM3 bsf PWM4 bsf PWM5 bsf PWM6 bsf PWM7 bsf PWM8 RETFIE FAST ;quit interrupt, automatic context saving (W, STATUS, BSR) updatePWM ;5*8 = 40 cycles < 3us update_pwm cptPWM1, PWM1 update_pwm cptPWM2, PWM2 update_pwm cptPWM3, PWM3 update_pwm cptPWM4, PWM4 update_pwm cptPWM5, PWM5 update_pwm cptPWM6, PWM6 update_pwm cptPWM7, PWM7 update_pwm cptPWM8, PWM8 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 btfsc spiReg, 7 bsf SDIO nop SCLK_PULSE rlcf spiReg, f decfsz cptBit, f goto boucleWriteReg; return ;********************************* ; spi_read ; sortie: ; spiReg: valeur du registre lu ;********************************* spi_read bsf SDIO_TRIS ;SDIO input movlwf .8, cptBit clrf spiReg bcf STATUS, C boucleReadReg rlcf spiReg, f btfsc SDIO bsf spiReg, 0 SCLK_PULSE decfsz cptBit, f goto boucleReadReg bcf SDIO_TRIS ;SDIO output return ;************** ; read_packet ;************** read_packet SPI_ON movlwf 0x45, spiReg call spi_write call spi_read ;sync movff spiReg, 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 <- 0x2AC57554 ??? useful --> YES !!! 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 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 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 swapf txid0, w ;substract txid0 high nibble from channel number andlw 0x0F subwf channel, f return ;************** ;************** ; MAIN PROGRAM ;************** ;************** START ;config oscillateur interne à 16Mhz avec PLLx4 movlwf 0x70, OSCCON ;disable analog inputs to enable digital inputs clrf ANSEL clrf ANSELH ;config tris port A,B,C bcf LED_TRIS ;LED digital output bcf CS_RX_TRIS ;CS RX A7105 digital output bcf CS_GYRO_TRIS ;CS GYRO L3G4200 digital output bcf SDIO_TRIS ;SDIO digital output bcf SCLK_TRIS ;SCLK digital output bcf PWM1_TRIS ;PWM1 digital output bcf PWM2_TRIS ;PWM2 digital output bcf PWM3_TRIS ;PWM3 digital output bcf PWM4_TRIS ;PWM4 digital output bcf PWM5_TRIS ;PWM5 digital output bcf PWM6_TRIS ;PWM6 digital output bcf PWM7_TRIS ;PWM7 digital output bcf PWM8_TRIS ;PWM8 digital output ;config port A,B,C clrf PORTA clrf PORTB clrf PORTC bsf LED ;LED ON bsf CS_RX ;CS RX default HIGH bsf CS_GYRO ;CS GYRO default HIGH bsf SDIO ;SDIO default HIGH bcf SCLK ;SCLK default LOW ;set timer2 ( pwm interrupt 100khz ) ;8 bits timer with comparator register movlwf .159, PR2 ; Timer2 comparator register => 16Mhz/1/(159+1)/1=100khz clrf T2CON ; reset timer2 control register 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 ?) ;set timer3 ( count time-out delay ) ;16 bit timer clrf T3CON ; reset timer3 control register bsf T3CON, RD16 ; set TMR3 bsf T3CON, T3CKPS1 ; 1:8 prescaler (0.5us period) bsf T3CON, T3CKPS0 ; 1:8 prescaler (0.5us period) bcf PIR2, TMR3IF ; clear flag interrupt timer3 ;init pwm values movlw .128 movwf valPWM1 movwf valPWM2 movwf valPWM3 movwf valPWM4 movwf valPWM5 movwf valPWM6 movwf valPWM7 movwf valPWM8 ;start timer 2 (pwm) bsf INTCON, GIE ; enable global interrupt bsf T2CON, TMR2ON ; timer2 on => starts pwm ;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 ;read ID A7105 -> id0..id3 SPI_ON movlwf 0x46, spiReg call spi_write call spi_read movlwf spiReg, id0 call spi_read movlwf spiReg, id1 call spi_read movlwf spiReg, id2 call spi_read movlwf spiReg, id3 SPI_OFF ;------------------------------------------------------------------ ;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) ;init first read packet 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 .2000 ;start timer3 for 2ms time out ;************ ; MAIN LOOP ;************ ;bsf LED ;for debug... mainLoop ;test time-out rx packet ? btfss PIR2, TMR3IF ;test time-out 2ms ? goto testPacketavailable ;no -> test packet available ;time-out ;bcf LED ;led off call readNextChannelNumber ;skip next channel because we probably allready missed it nop 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 .2000 ;start timer3 for 2ms time out goto readGyro ;packet available ? testPacketavailable btfsc GIO ;test packet available ? goto readGyro ;no -> read gyro ;read Packet ;yes -> read packet spi_read_ladr 0x00 ;read "mode register" btfsc spiReg, 5 ;test CRC (bit5) goto readGyro ;not valid -> read gyro btfsc spiReg, 6 ;test CEF (bit6) goto readGyro ;not valid -> read gyro call read_packet ;read RX packet movf txid0, w ;good TX ID0 ? cpfseq id0 goto readGyro movf txid1, w ;good TX ID1 ? cpfseq id1 goto readGyro movf txid2, w ;good TX ID2 ? cpfseq id2 goto readGyro movf txid3, w ;good TX ID3 ? cpfseq id3 goto readGyro ;bsf LED ;led ON = rx ok readGyro computePWM goto mainLoop ;********************************************************** ;*********************** 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