mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
Tools: add PIC assembly code for RCInput_UART
This patch adds the assembly source code for the PIC firmware responsible for processing PWM input from the RC Receiver and pushing the values to the serial output.
This commit is contained in:
Fabio Mello
Andrew Tridgell
parent
d16a924a15
commit
8110104361
359
Tools/Linux_HAL_Essentials/RCInput_UART/pic_firmware.asm
Normal file
359
Tools/Linux_HAL_Essentials/RCInput_UART/pic_firmware.asm
Normal file
@ -0,0 +1,359 @@
|
|||||||
|
#include <p16f690.inc>
|
||||||
|
|
||||||
|
list p=16f690
|
||||||
|
|
||||||
|
__config _INTRC_OSC_NOCLKOUT & _WDT_OFF & _PWRTE_ON & _MCLRE_OFF & _CP_OFF & _CPD_OFF & _BOR_ON & _IESO_OFF & _FCMEN_OFF
|
||||||
|
|
||||||
|
;
|
||||||
|
; This program will read until eight pwm inputs and will send its values
|
||||||
|
; through serial port at 115200bps.
|
||||||
|
;
|
||||||
|
;
|
||||||
|
; +5V -| VDD VSS |- GND
|
||||||
|
; input 6 -| RA5 RA0 |- input 1
|
||||||
|
; input 5 -| RA4 RA1 |- input 2
|
||||||
|
; input 4 -| RA3 RA2 |- input 3
|
||||||
|
; output 4 -| RC5 RC0 |- n. c.
|
||||||
|
; output 3 -| RC4 RC1 |- status led
|
||||||
|
; output 2 -| RC3 RC2 |- output 1
|
||||||
|
; n.c. -| RC6 RB4 |- input 8
|
||||||
|
; n.c. -| RC7 RB5 |- rx
|
||||||
|
; tx* -| RB7 RB6 |- input 7
|
||||||
|
;
|
||||||
|
;
|
||||||
|
; * In order to keep IO voltage at 3.3V use a voltage divider on TX.
|
||||||
|
;
|
||||||
|
; PS: outputs are not covered by this program yet.
|
||||||
|
;
|
||||||
|
|
||||||
|
; The INPUT_NUM is attached to input_mask and the
|
||||||
|
; interruption. Do not change this value without
|
||||||
|
; changing the interruption and mask.
|
||||||
|
#define INPUT_NUM 8
|
||||||
|
#define MAGIC 0x55AA
|
||||||
|
|
||||||
|
cblock 0x020
|
||||||
|
; input_value, input_rise and input_mirror
|
||||||
|
; are at the bank's begining to make easy
|
||||||
|
; the exchange of data between them.
|
||||||
|
input_value:(INPUT_NUM * 2)
|
||||||
|
input_previous
|
||||||
|
input_current
|
||||||
|
input_changed
|
||||||
|
input_mask
|
||||||
|
input_updated
|
||||||
|
sent_update
|
||||||
|
endc
|
||||||
|
|
||||||
|
cblock 0x72
|
||||||
|
save_w
|
||||||
|
save_status
|
||||||
|
save_fsr
|
||||||
|
timer_l
|
||||||
|
timer_h
|
||||||
|
count
|
||||||
|
endc
|
||||||
|
|
||||||
|
cblock 0x0a0
|
||||||
|
; see input_value
|
||||||
|
input_rise:(INPUT_NUM * 2)
|
||||||
|
endc
|
||||||
|
|
||||||
|
cblock 0x120
|
||||||
|
; see input_value
|
||||||
|
input_mirror:(INPUT_NUM * 2)
|
||||||
|
endc
|
||||||
|
|
||||||
|
; Reset Vector
|
||||||
|
org 0x0000
|
||||||
|
nop
|
||||||
|
nop
|
||||||
|
nop
|
||||||
|
goto Main
|
||||||
|
|
||||||
|
; Interrupts Vector
|
||||||
|
org 0x0004
|
||||||
|
; save the current context
|
||||||
|
movwf save_w
|
||||||
|
swapf STATUS, w
|
||||||
|
clrf STATUS
|
||||||
|
movwf save_status
|
||||||
|
movf FSR, w
|
||||||
|
movwf save_fsr
|
||||||
|
|
||||||
|
; it's a input event
|
||||||
|
btfss INTCON, RABIF
|
||||||
|
goto IntInputDone
|
||||||
|
|
||||||
|
; capture timer 1
|
||||||
|
movf TMR1H, w
|
||||||
|
movwf timer_h
|
||||||
|
movf TMR1L, w
|
||||||
|
movwf timer_l
|
||||||
|
movf TMR1H, w
|
||||||
|
subwf timer_h, w
|
||||||
|
btfsc STATUS, Z
|
||||||
|
goto IntTMR1Done
|
||||||
|
movf TMR1H, w
|
||||||
|
movwf timer_h
|
||||||
|
movf TMR1L, w
|
||||||
|
movwf timer_l
|
||||||
|
IntTMR1Done:
|
||||||
|
|
||||||
|
; capture the current input state
|
||||||
|
movf PORTA, w
|
||||||
|
btfsc PORTB, RB6
|
||||||
|
iorlw (1<<6)
|
||||||
|
btfsc PORTB, RB4
|
||||||
|
iorlw (1<<7)
|
||||||
|
|
||||||
|
bcf INTCON, RABIF
|
||||||
|
|
||||||
|
; identify the changed inputs
|
||||||
|
movwf input_current
|
||||||
|
xorwf input_previous, w
|
||||||
|
movwf input_changed
|
||||||
|
movf input_current, w
|
||||||
|
movwf input_previous
|
||||||
|
|
||||||
|
; prepare for loop
|
||||||
|
movlw .1
|
||||||
|
movwf input_mask
|
||||||
|
movlw input_rise
|
||||||
|
movwf FSR
|
||||||
|
goto IntInputLoop
|
||||||
|
|
||||||
|
IntInputNext:
|
||||||
|
incf FSR, f
|
||||||
|
|
||||||
|
IntInputNextHalf:
|
||||||
|
; ... and ensure bank 0 selection
|
||||||
|
incf FSR, f
|
||||||
|
bsf FSR, 7
|
||||||
|
|
||||||
|
; done if no more channels
|
||||||
|
bcf STATUS, C
|
||||||
|
rlf input_mask, f
|
||||||
|
btfsc STATUS, C
|
||||||
|
goto IntInputDone
|
||||||
|
|
||||||
|
IntInputLoop:
|
||||||
|
; skip if channel not changed
|
||||||
|
movf input_mask, w
|
||||||
|
andwf input_changed, w
|
||||||
|
btfsc STATUS, Z
|
||||||
|
goto IntInputNext
|
||||||
|
|
||||||
|
; check if it is raising or falling
|
||||||
|
andwf input_current, w
|
||||||
|
btfss STATUS, Z
|
||||||
|
goto IntInputRise
|
||||||
|
|
||||||
|
; calculate and move the lsb to input_value
|
||||||
|
movf INDF, w
|
||||||
|
subwf timer_l, w
|
||||||
|
bcf FSR, 7
|
||||||
|
movwf INDF
|
||||||
|
|
||||||
|
; calculate and move the msb to input_value
|
||||||
|
bsf FSR, 7
|
||||||
|
incf FSR, f
|
||||||
|
movf INDF, w
|
||||||
|
btfss STATUS, C
|
||||||
|
incf INDF, w
|
||||||
|
subwf timer_h, w
|
||||||
|
bcf FSR, 7
|
||||||
|
movwf INDF
|
||||||
|
|
||||||
|
goto IntInputNextHalf
|
||||||
|
|
||||||
|
IntInputRise:
|
||||||
|
; tell main loop we have all channels
|
||||||
|
btfsc input_mask, 1
|
||||||
|
incf input_updated, f
|
||||||
|
|
||||||
|
; save the rise instant
|
||||||
|
movf timer_l, w
|
||||||
|
movwf INDF
|
||||||
|
incf FSR, f
|
||||||
|
movf timer_h, w
|
||||||
|
movwf INDF
|
||||||
|
goto IntInputNextHalf
|
||||||
|
|
||||||
|
IntInputDone:
|
||||||
|
|
||||||
|
; restore the previous context
|
||||||
|
movf save_fsr, w
|
||||||
|
movwf FSR
|
||||||
|
swapf save_status, w
|
||||||
|
movwf STATUS
|
||||||
|
swapf save_w, f
|
||||||
|
swapf save_w, w
|
||||||
|
retfie
|
||||||
|
|
||||||
|
Main:
|
||||||
|
; bank 0
|
||||||
|
clrf STATUS
|
||||||
|
|
||||||
|
clrf PORTA
|
||||||
|
clrf PORTB
|
||||||
|
clrf PORTC
|
||||||
|
|
||||||
|
; bank 1
|
||||||
|
bsf STATUS, RP0
|
||||||
|
|
||||||
|
movlw (b'111'<<IRCF0)
|
||||||
|
movwf OSCCON
|
||||||
|
|
||||||
|
movlw (0<<NOT_RABPU)
|
||||||
|
movwf OPTION_REG
|
||||||
|
|
||||||
|
; ,------ input 6
|
||||||
|
; |,----- input 5
|
||||||
|
; ||,---- input 4
|
||||||
|
; |||,--- input 3
|
||||||
|
; ||||,-- input 2
|
||||||
|
; |||||,- input 1
|
||||||
|
movlw b'11111111'
|
||||||
|
movwf TRISA
|
||||||
|
movwf WPUA
|
||||||
|
movwf IOCA
|
||||||
|
|
||||||
|
; ,-------- tx
|
||||||
|
; |,------- input 6
|
||||||
|
; ||,------ rx
|
||||||
|
; |||,----- input 7
|
||||||
|
movlw b'01111111'
|
||||||
|
movwf TRISB
|
||||||
|
|
||||||
|
; ,------ output 4
|
||||||
|
; |,----- output 3
|
||||||
|
; ||,---- output 2
|
||||||
|
; |||,--- output 1
|
||||||
|
; ||||,-- status
|
||||||
|
; |||||,- n. c.
|
||||||
|
movlw b'11000001'
|
||||||
|
movwf TRISC
|
||||||
|
|
||||||
|
movlw (1<<TXEN|1<<BRGH)
|
||||||
|
movwf TXSTA
|
||||||
|
|
||||||
|
movlw (1<<BRG16)
|
||||||
|
movwf BAUDCTL
|
||||||
|
|
||||||
|
movlw .16
|
||||||
|
movwf SPBRG
|
||||||
|
clrf SPBRGH
|
||||||
|
|
||||||
|
; Bank 2
|
||||||
|
bcf STATUS, RP0
|
||||||
|
bsf STATUS, RP1
|
||||||
|
|
||||||
|
clrf ANSEL
|
||||||
|
clrf ANSELH
|
||||||
|
|
||||||
|
; ,------- input 6
|
||||||
|
; | ,----- input 7
|
||||||
|
movlw b'01010000'
|
||||||
|
movwf WPUB
|
||||||
|
movwf IOCB
|
||||||
|
|
||||||
|
movlw (1<<TXEN|1<<BRGH)
|
||||||
|
movwf TXSTA
|
||||||
|
|
||||||
|
; bank 0
|
||||||
|
clrf STATUS
|
||||||
|
|
||||||
|
; clear the first two banks
|
||||||
|
movlw 0x020
|
||||||
|
call ClearBank
|
||||||
|
movlw 0x0A0
|
||||||
|
call ClearBank
|
||||||
|
|
||||||
|
movlw (1<<SPEN|1<<CREN)
|
||||||
|
movwf RCSTA
|
||||||
|
|
||||||
|
movlw (1<<T1CKPS0|1<<TMR1ON)
|
||||||
|
movwf T1CON
|
||||||
|
|
||||||
|
movlw (1<<GIE|1<<PEIE|1<<RABIE)
|
||||||
|
movwf INTCON
|
||||||
|
|
||||||
|
MainLoop:
|
||||||
|
; loop if no updates
|
||||||
|
movf sent_update, w
|
||||||
|
subwf input_updated, w
|
||||||
|
btfsc STATUS, Z
|
||||||
|
goto MainLoop
|
||||||
|
|
||||||
|
; prepare for mirroring
|
||||||
|
addwf sent_update, f
|
||||||
|
movlw (INPUT_NUM * 2)
|
||||||
|
movwf count
|
||||||
|
movlw input_value
|
||||||
|
movwf FSR
|
||||||
|
|
||||||
|
MirrorLoop:
|
||||||
|
movf INDF, w
|
||||||
|
bsf STATUS, IRP
|
||||||
|
movwf INDF
|
||||||
|
bcf STATUS, IRP
|
||||||
|
incf FSR, f
|
||||||
|
decfsz count, f
|
||||||
|
goto MirrorLoop
|
||||||
|
|
||||||
|
; restart if there was update while mirroring
|
||||||
|
movf input_updated, w
|
||||||
|
subwf sent_update, w
|
||||||
|
btfss STATUS, Z
|
||||||
|
goto MainLoop
|
||||||
|
|
||||||
|
; finally send the captured values
|
||||||
|
; send a magic number to sync
|
||||||
|
movlw low(MAGIC)
|
||||||
|
call Send
|
||||||
|
movlw high(MAGIC)
|
||||||
|
call Send
|
||||||
|
|
||||||
|
; prepare for send loop
|
||||||
|
movlw (INPUT_NUM * 2)
|
||||||
|
movwf count
|
||||||
|
movlw low(input_mirror)
|
||||||
|
bsf STATUS, IRP
|
||||||
|
movwf FSR
|
||||||
|
|
||||||
|
SendLoop:
|
||||||
|
movf INDF, w
|
||||||
|
call Send
|
||||||
|
incf FSR, f
|
||||||
|
decfsz count, f
|
||||||
|
goto SendLoop
|
||||||
|
|
||||||
|
bcf STATUS, IRP
|
||||||
|
goto MainLoop
|
||||||
|
|
||||||
|
Send:
|
||||||
|
btfss PIR1, TXIF
|
||||||
|
goto $ - 1
|
||||||
|
movwf TXREG
|
||||||
|
return
|
||||||
|
|
||||||
|
ClearBank:
|
||||||
|
; clears the gpr bank pointed by w plus irp,
|
||||||
|
; but preserves the common 16 bytes.
|
||||||
|
|
||||||
|
; ensure the begining of the bank
|
||||||
|
andlw b'10100000'
|
||||||
|
movwf FSR
|
||||||
|
movlw .80
|
||||||
|
movwf count
|
||||||
|
|
||||||
|
ClearBankLoop:
|
||||||
|
clrf INDF
|
||||||
|
incf FSR, f
|
||||||
|
decfsz count, f
|
||||||
|
goto ClearBankLoop
|
||||||
|
return
|
||||||
|
|
||||||
|
end
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user