diff --git a/Tools/Linux_HAL_Essentials/RCInput_UART/pic_firmware.asm b/Tools/Linux_HAL_Essentials/RCInput_UART/pic_firmware.asm new file mode 100644 index 0000000000..6be2b51c25 --- /dev/null +++ b/Tools/Linux_HAL_Essentials/RCInput_UART/pic_firmware.asm @@ -0,0 +1,359 @@ +#include + + 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'<