;********************************************************************** ;Project name : PIC 16X84 BASED ULTRASONIC RANGEFINDER WITH LCD SUPPORT ; Author : Nikola Pavkovic ; e-mail : NiX@LinuxFreak.com ;********************************************************************** ; How this works ? Simply ! ; PIC sends a packet of three 40kHz pulses trough the ultrasonic ; trasducer, and waits for echo on INT pin which is connected to ; the ultrasonic receiver (see the shematic part). ; Then PIC stores the time in buffer.This happens eight times, and ; then the medium value of the eight results is calculated, and sent ; to LCD module. After the the result is displayed, a new measuring ; cycle is executed. ;********************************************************************** device pic16f84 config CP=off,WDT=on,PWRT=off,OSC=xt include "picmac.h" include "pic16c84.h" include "lcdsonar.h" include "math16.inc" ORG 0x000 goto Start ORG 0x004 goto Interrupt ;***************************************************************** ; Function : Start ; ; Description : This function is executed when the device is ; turned on. ;***************************************************************** Start: clrwdt call Init ; Do some init stuff call LCD_Init ; Initialize the LCD ; EndFunction : Main ********************************************* ;***************************************************************** ; Function : MainLoopSetup ; ; Description : This function prepares system for MainLoop ;***************************************************************** MainLoopSetup clrwdt call ProgIdleTimer ; Set TMR0 parameters call StartInt ; Start measuring ; EndFunction : MainLoopSetup ************************************ ;***************************************************************** ; Function : MainLoop ; ; Description : This is the main function ;***************************************************************** MainLoop movf LowByteDelayOld,W xorwf LowByteDelay,W btfsc STATUS,Z ; Check if new result available goto MainLoop ; NO: loop clrf INTCON call UnProgIdleTimer ; ***** movf LowByteDelay,W movwf LowByteDelayOld call Calculate_Result ; Calculate call Display_Result ; and diaplay result on LCD goto MainLoopSetup ; ...and again... ; EndFunction : MainLoop ***************************************** ;***************************************************************** ; Interrupt service routine ;***************************************************************** Interrupt: movwf SaveWReg swapf STATUS,w ; movwf SaveStatus ; Save STATUS register btfsc INTCON,RTIF ; Check type of int goto TimerInt btfsc INTCON,INTF goto Int RestoreIntStatus: swapf SaveStatus,w movwf STATUS ; restore STATUS register swapf SaveWReg, F ; save W register swapf SaveWReg,w ; restore W register retfie ;***************************************************************** ; Function : TimerInt ; ; Description : When TMR0 overflow occures, this function is to ; be executed. It increases the Counter2mS register (timer ; overflow occures every 2mS), and checks if elapsed time (since ; the last pulse packet was sent) reached the point where no more ; echo expected (device measuring range is exceeded). If so, ; a new packet of pulses is being sent. ;***************************************************************** TimerInt: bcf INTCON,RTIF clrwdt incf Counter2mS, f movf Counter2mS, w sublw InternDelayTime ; Check if new pulse packet btfsc STATUS,Z ; is needed call SendPulse ; Yes: Send pulses. goto RestoreIntStatus ; EndFunction : TimerInt ***************************************** ;***************************************************************** ; Function : Int ; ; Description : This function is executed when echo is detected. ; It stores the TMR0 value (representing the LSB) into buffer. ; When eight measures are made the middle value for LSB is being ; calculated, and the MSB and LSM are stored to HighByteDelay and ; LowByteDelay ;***************************************************************** Int: bcf INTCON,INTF bcf INTCON,INTE movlw BufferAdress ; Store 8 LSBytes in buffer addwf IndexLowByte, w ; The MSB is Counter2mS and the LSB is RTCC movwf FSR ; This two bytes count the time between movf RTCC,w ; the first transmited pulse and the first movwf INDF ; received pulse (echo). incf IndexLowByte, w andlw 0x07 ; Check if was made 8 measurements. movwf IndexLowByte btfsc STATUS,Z ; Yes: Make the medium value for the LSB call LoadLowByte movf Counter2mS,w movwf HighByteDelay ; Store MSB. goto RestoreIntStatus ; EndFunction : Int ********************************************** ;***************************************************************** ; Function : Init ; ; Description : Initialization stuff here... ;***************************************************************** Init movlw 0x0C movwf FSR ClearRegs: ; Clear all data RAM. clrf INDF incf FSR, f movlw 0x2F subwf FSR, w bnz ClearRegs clrf PORTB clrf PORTA bsf STATUS,RP0 ; pag. 1 movlw 0x00 movwf TRISB movwf TRISA bcf STATUS,RP0 ; pag. 0 clrf PORTA clrf PORTB bsf PORTA,Out1 ; Set initial values on outputs. return ; EndFunction : Init ********************************************* ;***************************************************************** ; Function : (Un)ProgIdleTimer ; ; Description : This function (un)programs the TMR0 module. ;***************************************************************** UnProgIdleTimer bsf STATUS,RP0 movlw 0xFF movwf OPTION_REG bcf STATUS,RP0 return ProgIdleTimer ; Program the TMR0 bsf STATUS,RP0 ; pag. 1 bcf OPTIO,RTS ; TMR0 increments on internal clock bcf OPTIO,PSA ; at 8 uS ( prescaler 1:8 ). bcf OPTIO,PS2 bsf OPTIO,PS1 bcf OPTIO,PS0 bsf OPTIO,INTEDG bcf STATUS,RP0 ; pag. 0 return ; EndFunction : (Un)ProgIdleTimer ******************************** ;***************************************************************** ; Function : StartInt ; ; Description : This function enables TMR0 interrupt ;***************************************************************** StartInt clrf INTCON bsf INTCON,RTIE ; Enable TMR0 interrupt. bsf INTCON,GIE return ; EndFunction : StrtInt ****************************************** ;***************************************************************** ; Function : LoadLowByte ; ; Description : This function calculates the middle value for LSB ;***************************************************************** LoadLowByte ; If was made 8 measurements make the ; medium value for LSB. movlw BufferAdress movwf FSR clrf TempLowByte clrf TempHighByte movlw 8 movwf IndexLowByte LoopAdd: ; Add the 8 values. movf INDF, w addwf TempLowByte, f btfsc STATUS,C incf TempHighByte, f incf FSR, f decfsz IndexLowByte, f goto LoopAdd movlw 3 movwf IndexLowByte LoopShift: bcf STATUS,C rrf TempHighByte, f rrf TempLowByte, f decfsz IndexLowByte, f goto LoopShift movf TempLowByte, w movwf LowByteDelay return ; EndFunction : LoadLowByte ************************************** ;***************************************************************** ; Function : SendPulse ; ; Description : This function clears Counter2mS reg, sends three ; pulses at 40 kHz trough ultrasonic transducer, and enables the ; RB0 interrupt. ;***************************************************************** SendPulse ; Send 3 pulses at 40 kHz ( Yes, only 3 ). clrf Counter2mS movlw 3 movwf PulseCounter PulseLoop: nop nop nop nop nop nop nop bcf PORTA,Out1 bsf PORTA,Out2 nop nop nop nop nop nop nop nop nop nop nop nop bsf PORTA,Out1 bcf PORTA,Out2 decfsz PulseCounter, f goto PulseLoop bcf INTCON,INTF bsf INTCON,INTE ; Enable INT interrupt. return ; ( wait echo on INT pin ). ; EndFunction : SendPulse **************************************** ;***************************************************************** ; Function : LCD_Init ; ; Description : LCD initialization routine. ;***************************************************************** LCD_Init bsf PORTA,EchoDisable ; Disable reciever signal on RB0, ; because we need the whole PORTB ; for LCD control bsf STATUS,RP0 ; Bank1 movlw 0x00 ; RB<0..7> are outputs movwf TRISB bcf STATUS,RP0 ; Bank0 movlw 0xFF call Delay_ms ; Wait a while call FUNCTION_SET movlw 0xFF call Delay_ms call DISPLAY_CONTROL movlw 0xFF call Delay_ms bsf STATUS,RP0 ; Bank1 movlw 0x01 ; RB<1..7> are outputs, RB0 is input movwf TRISB bcf STATUS,RP0 ; Bank0 bcf PORTA,EchoDisable ; Enable reciever signal on RB0 return ; EndFunction : LCD_Init****************************************** ;***************************************************************** ; Function : Display_Result ; ; Description : This function displays the measured value (stored ; in ONES, TENS, HUNDREDS and THOUSENDS registers in ASCII) on LCD ;***************************************************************** Display_Result bsf PORTA,EchoDisable ; Disable reciever signal on RB0 bsf STATUS,RP0 ; Bank1 movlw 0x00 ; RB<0..7> are outputs movwf TRISB bcf STATUS,RP0 ; Bank0 call CURSOR_HOME call ENTRY_MODE_SET movlw 'D' call WRITE_CHAR movlw 'e' call WRITE_CHAR movlw 'p' call WRITE_CHAR movlw 't' call WRITE_CHAR movlw 'h' call WRITE_CHAR movlw ':' call WRITE_CHAR movlw ' ' call WRITE_CHAR movf THOUSANDS, w call WRITE_CHAR movf HUNDREDS, w call WRITE_CHAR movlw 'm' call WRITE_CHAR movlw ' ' call WRITE_CHAR movf TENS, w call WRITE_CHAR movf ONES, w call WRITE_CHAR movlw 'c' call WRITE_CHAR movlw 'm' call WRITE_CHAR bsf STATUS,RP0 ; Bank1 movlw 0x01 ; RB<1..7> are outputs, RB0 is input movwf TRISB bcf STATUS,RP0 ; Bank0 bcf PORTA,EchoDisable ; Enable reciever signal on RB0 return ; EndFunction : Display_Result ****************************************** ;************************************************************************ ; Some LCD control routines ;************************************************************************ FUNCTION_SET bcf PORTA, DISP_RS bcf PORTA, DISP_E movlw 0x30 movwf PORTB call PULSE_E return DISPLAY_CONTROL bcf PORTA, DISP_RS bcf PORTA, DISP_E movlw 0x0E movwf PORTB call PULSE_E return ENTRY_MODE_SET bcf PORTA, DISP_RS bcf PORTA, DISP_E movlw 0x06 movwf PORTB call PULSE_E return WRITE_CHAR movwf PORTB bsf PORTA, DISP_RS call PULSE_E bcf PORTA, DISP_RS return CLEAR_DISPLAY bcf PORTA, DISP_RS movlw 0x01 movwf PORTB call PULSE_E return CURSOR_HOME bcf PORTA, DISP_RS movlw 0x02 movwf PORTB call PULSE_E return PULSE_E bsf PORTA, DISP_E movlw 0x02 call Delay_ms bcf PORTA, DISP_E movlw 0x02 call Delay_ms return ;***************************************************************** ; Function : Delay_ms ; ; Description : This function makes a delay of a number of ; milliseconds stored in W reg. ;***************************************************************** Delay_ms movwf MSTIMR ; Store the number of milliseconds Delay_ms1 movlw 0xF9 addlw -1 skipz goto $-2 ; 1mS delay loop decfsz MSTIMR,f ; goto Delay_ms1 ; main delay loop return ; EndFunction : Delay_ms ******************************************* ;***************************************************************** ; Function : Calculate_Result ; ; Description : This function converts the numbers in HighByteDelay ; and LowByteDelay regs to floats and calculats the distance as a ; function of echo delay time: ; Distance[cm]=(256*HighByteDelay+LowByteDelay)*15/109-4 ; Regs in comments with a '*' sign are actually register triples. ; Each triplet contains a number stored in floating point format. ;***************************************************************** Calculate_Result movlw 0x40 call To_Float call A_to_Temp ; Temp* = 64 movlw 0x04 call To_Float call A_to_B ; B* = 4 call Temp_to_A ; A* = 64, call FPM24 ; Multiply -> A*=256 call A_to_Temp ; Temp* = 256 movf HighByteDelay, w call To_Float call A_to_B call Temp_to_A call FPM24 call A_to_Temp ; Temp*=256*H movf LowByteDelay, w call To_Float call A_to_B call Temp_to_A call FPA24 call A_to_Temp ; Temp*=256*H+L movlw 0x0F call To_Float call A_to_B call Temp_to_A call FPM24 call A_to_Temp ; Temp*=(256*H+L)*15 movlw 0x6D ;109 call To_Float call A_to_B call Temp_to_A call FPD24 call A_to_Temp ; Temp*=(256*H+L)*15/109 call Temp_to_A call INT24 ; convert to integer call A_to_Temp ; TempArg0 = MSB, TempArg1 = LSB call Convert_to_Decimal call Prepare_ASCII return A_to_Temp movf AARGB0, w movwf TempArg0 movf AARGB1, w movwf TempArg1 movf AEXP, w movwf TempExp return A_to_B movf AARGB0, w movwf BARGB0 movf AARGB1, w movwf BARGB1 movf AEXP, w movwf BEXP return Temp_to_A movf TempArg0, w movwf AARGB0 movf TempArg1, w movwf AARGB1 movf TempExp, w movwf AEXP return To_Float movwf AARGB1 clrf AARGB0 call FLO24 ;convert W to float return ; EndFunction : Calculate_Result ********************************* ;***************************************************************** ; Function : Prepare_ASCII ; ; Description : Adds 0x30 to each digit register to get ASCII codes ;***************************************************************** Prepare_ASCII movlw 0x30 addwf ONES, f addwf TENS, f addwf HUNDREDS, f addwf THOUSANDS, f ; we add 0x30 to get ASCII return ; EndFunction : Prepare_ASCII ************************************ ;***************************************************************** ; Function : Convert_to_Decimal ; ; Description : This function converts the 16bit binary integer ; contained by TempArg0 and TempArg1 to BASE10 ;***************************************************************** Convert_to_Decimal clrf ONES clrf TENS clrf HUNDREDS clrf THOUSANDS clrf Tempreg clrf Tempreg1 movf TempArg1, w andlw 0x0F movwf Tempreg movwf Tempreg1 movlw 0x01 movwf MULTIPLIER call ADD_1 swapf TempArg1, w andlw 0x0F movwf Tempreg movwf Tempreg1 movlw 0x10 movwf MULTIPLIER call ADD_1 movf TempArg0, w andlw 0x0F movwf Tempreg movwf Tempreg1 movlw 0x20 movwf MULTIPLIER call ADD_1 swapf TempArg0, w andlw 0x0F movwf Tempreg movwf Tempreg1 movlw 0x30 movwf MULTIPLIER call ADD_1 return ADD_1 movf Tempreg, f btfsc STATUS,Z return incf ONES, f movlw 0x0A subwf ONES, w btfsc STATUS,C call ADD_10 decf Tempreg, f btfss STATUS,Z goto ADD_1 movf Tempreg1, w movwf Tempreg decf MULTIPLIER, f btfss STATUS,Z goto ADD_1 return ADD_10 incf TENS, f movlw 0x0A subwf TENS, w btfsc STATUS,C call ADD_100 clrf ONES return ADD_100 incf HUNDREDS, f movlw 0x0A subwf HUNDREDS, w btfsc STATUS,C call ADD_1000 clrf TENS return ADD_1000 incf THOUSANDS, f clrf HUNDREDS return ; EndFunction : Convert_to_Decimal ****************************** ;********************************************************************************************** ; Below here : Floating point math routines downloaded from http://www.microchip.com/ ;********************************************************************************************** ;********************************************************************************************** ; Integer to float conversion ; Input: 16 bit 2's complement integer right justified in AARGB0, AARGB1 ; Use: CALL FLO1624 or CALL FLO24 ; Output: 24 bit floating point number in AEXP, AARGB0, AARGB1 FLO1624 FLO24 MOVLW D'15'+EXPBIAS ; initialize exponent and add bias MOVWF EXP MOVF AARGB0,W MOVWF SIGN BTFSS AARGB0,MSB ; test sign GOTO NRM2424 COMF AARGB1,F ; if < 0, negate and set MSB in SIGN COMF AARGB0,F INCF AARGB1,F BTFSC STATUS,Z INCF AARGB0,F ;********************************************************************************************** ; Normalization routine ; Input: 24 bit unnormalized floating point number in AEXP, AARGB0, AARGB1, ; with sign in SIGN,MSB and other bits zero. ; Use: CALL NRM2424 or CALL NRM24 ; Output: 24 bit normalized floating point number in AEXP, AARGB0, AARGB1 NRM2424 NRM24 CLRF TEMP ; clear exponent decrement MOVF AARGB0,W ; test if highbyte=0 BTFSS STATUS,Z GOTO NORM2424 MOVF AARGB1,W ; if so, shift 8 bits by move MOVWF AARGB0 BTFSC STATUS,Z ; if highbyte=0, result=0 GOTO RES024 CLRF AARGB1 BSF TEMP,3 NORM2424 MOVF TEMP,W SUBWF EXP,F BTFSS STATUS,Z BTFSS STATUS,C GOTO SETFUN24 BCF STATUS,C ; clear carry bit NORM2424A BTFSC AARGB0,MSB ; if MSB=1, normalization done GOTO FIXSIGN24 RLF AARGB1,F ; otherwise, shift left and RLF AARGB0,F ; decrement EXP DECFSZ EXP,F GOTO NORM2424A GOTO SETFUN24 ; underflow if EXP=0 FIXSIGN24 BTFSS SIGN,MSB BCF AARGB0,MSB ; clear explicit MSB if positive RETLW 0 RES024 CLRF AARGB0 ; result equals zero CLRF AARGB1 CLRF AARGB2 ; clear extended byte CLRF EXP RETLW 0 ;********************************************************************************************** ; Normalization routine ; Input: 32 bit unnormalized floating point number in AEXP, AARGB0, AARGB1, ; AARGB2, with sign in SIGN,MSB ; Use: CALL NRM3224 ; Output: 24 bit normalized floating point number in AEXP, AARGB0, AARGB1 NRM3224 CLRF TEMP ; clear exponent decrement MOVF AARGB0,W ; test if highbyte=0 BTFSS STATUS,Z GOTO NORM3224 MOVF AARGB1,W ; if so, shift 8 bits by move MOVWF AARGB0 MOVF AARGB2,W MOVWF AARGB1 CLRF AARGB2 BSF TEMP,3 ; increase decrement by 8 MOVF AARGB0,W ; test if highbyte=0 BTFSS STATUS,Z GOTO NORM3224 MOVF AARGB1,W ; if so, shift 8 bits by move MOVWF AARGB0 CLRF AARGB1 BCF TEMP,3 ; increase decrement by 8 BSF TEMP,4 MOVF AARGB0,W ; if highbyte=0, result=0 BTFSC STATUS,Z GOTO RES024 NORM3224 MOVF TEMP,W SUBWF EXP,F BTFSS STATUS,Z BTFSS STATUS,C GOTO SETFUN24 BCF STATUS,C ; clear carry bit NORM3224A BTFSC AARGB0,MSB ; if MSB=1, normalization done GOTO NRMRND3224 RLF AARGB2,F ; otherwise, shift left and RLF AARGB1,F ; decrement EXP RLF AARGB0,F DECFSZ EXP,F GOTO NORM3224A GOTO SETFUN24 ; underflow if EXP=0 NRMRND3224 BTFSC FPFLAGS,RND BTFSS AARGB1,LSB GOTO FIXSIGN24 BTFSS AARGB2,MSB ; round if next bit is set GOTO FIXSIGN24 INCF AARGB1,F BTFSC STATUS,Z INCF AARGB0,F BTFSS STATUS,Z ; has rounding caused carryout? GOTO FIXSIGN24 RRF AARGB0,F ; if so, right shift RRF AARGB1,F INCF EXP,F BTFSC STATUS,Z ; check for overflow GOTO SETFOV24 GOTO FIXSIGN24 ;********************************************************************************************** ;********************************************************************************************** ; Float to integer conversion ; Input: 24 bit floating point number in AEXP, AARGB0, AARGB1 ; Use: CALL INT2416 or CALL INT24 ; Output: 16 bit 2's complement integer right justified in AARGB0, AARGB1 INT2416 INT24 MOVF EXP,W ; test for zero argument BTFSC STATUS,Z RETLW 0x00 MOVF AARGB0,W ; save sign in SIGN MOVWF SIGN BSF AARGB0,MSB ; make MSB explicit MOVLW EXPBIAS+D'15' ; remove bias from EXP SUBWF EXP,F BTFSS EXP,MSB GOTO SETIOV16 COMF EXP,F INCF EXP,F MOVLW 8 ; do byte shift if EXP >= 8 SUBWF EXP,W BTFSS STATUS,C GOTO TSHIFT2416 MOVWF EXP RLF AARGB1,F ; rotate next bit for rounding MOVF AARGB0,W MOVWF AARGB1 CLRF AARGB0 MOVLW 8 ; do byte shift if EXP >= 8 SUBWF EXP,W BTFSS STATUS,C GOTO TSHIFT2416 MOVWF EXP RLF AARGB1,F ; rotate next bit for rounding CLRF AARGB1 MOVF EXP,W BTFSS STATUS,Z BCF STATUS,C GOTO SHIFT2416OK TSHIFT2416 MOVF EXP,W ; shift completed if EXP = 0 BTFSC STATUS,Z GOTO SHIFT2416OK SHIFT2416 BCF STATUS,C RRF AARGB0,F ; right shift by EXP RRF AARGB1,F DECFSZ EXP,F GOTO SHIFT2416 SHIFT2416OK BTFSC FPFLAGS,RND BTFSS AARGB1,LSB GOTO INT2416OK BTFSS STATUS,C ; round if next bit is set GOTO INT2416OK INCF AARGB1,F BTFSC STATUS,Z INCF AARGB0,F BTFSC AARGB0,MSB ; test for overflow GOTO SETIOV16 INT2416OK BTFSS SIGN,MSB ; if sign bit set, negate RETLW 0 COMF AARGB1,F COMF AARGB0,F INCF AARGB1,F BTFSC STATUS,Z INCF AARGB0,F RETLW 0 SETIOV16 BSF FPFLAGS,IOV ; set integer overflow flag BTFSS FPFLAGS,SAT ; test for saturation RETLW 0xFF ; return error code in WREG CLRF AARGB0 ; saturate to largest two's BTFSS SIGN,MSB ; complement 16 bit integer MOVLW 0xFF MOVWF AARGB0 ; SIGN = 0, 0x 7F FF MOVWF AARGB1 ; SIGN = 1, 0x 80 00 RLF SIGN,F RRF AARGB0,F RETLW 0xFF ; return error code in WREG ;********************************************************************************************** ;********************************************************************************************** ; Float to integer conversion ; Input: 24 bit floating point number in AEXP, AARGB0, AARGB1 ; Use: CALL INT2424 ; Output: 24 bit 2's complement integer right justified in AARGB0, AARGB1, AARGB2 INT2424 CLRF AARGB2 MOVF EXP,W ; test for zero argument BTFSC STATUS,Z RETLW 0x00 MOVF AARGB0,W ; save sign in SIGN MOVWF SIGN BSF AARGB0,MSB ; make MSB explicit MOVLW EXPBIAS+D'23' ; remove bias from EXP SUBWF EXP,F BTFSS EXP,MSB GOTO SETIOV24 COMF EXP,F INCF EXP,F MOVLW 8 ; do byte shift if EXP >= 8 SUBWF EXP,W BTFSS STATUS,C GOTO TSHIFT2424 MOVWF EXP RLF AARGB2,F ; rotate next bit for rounding MOVF AARGB1,W MOVWF AARGB2 MOVF AARGB0,W MOVWF AARGB1 CLRF AARGB0 MOVLW 8 ; do another byte shift if EXP >= 8 SUBWF EXP,W BTFSS STATUS,C GOTO TSHIFT2424 MOVWF EXP RLF AARGB2,F ; rotate next bit for rounding MOVF AARGB1,W MOVWF AARGB2 CLRF AARGB1 MOVLW 8 ; do another byte shift if EXP >= 8 SUBWF EXP,W BTFSS STATUS,C GOTO TSHIFT2424 MOVWF EXP RLF AARGB2,F ; rotate next bit for rounding CLRF AARGB2 MOVF EXP,W BTFSS STATUS,Z BCF STATUS,C GOTO SHIFT2424OK TSHIFT2424 MOVF EXP,W ; shift completed if EXP = 0 BTFSC STATUS,Z GOTO SHIFT2424OK SHIFT2424 BCF STATUS,C RRF AARGB0,F ; right shift by EXP RRF AARGB1,F RRF AARGB2,F DECFSZ EXP,F GOTO SHIFT2424 SHIFT2424OK BTFSC FPFLAGS,RND BTFSS AARGB2,LSB GOTO INT2424OK BTFSS STATUS,C GOTO INT2424OK INCF AARGB2,F BTFSC STATUS,Z INCF AARGB1,F BTFSC STATUS,Z INCF AARGB0,F BTFSC AARGB0,MSB ; test for overflow GOTO SETIOV24 INT2424OK BTFSS SIGN,MSB ; if sign bit set, negate RETLW 0 COMF AARGB0,F COMF AARGB1,F COMF AARGB2,F INCF AARGB2,F BTFSC STATUS,Z INCF AARGB1,F BTFSC STATUS,Z INCF AARGB0,F RETLW 0 IRES024 CLRF AARGB0 ; integer result equals zero CLRF AARGB1 CLRF AARGB2 RETLW 0 SETIOV24 BSF FPFLAGS,IOV ; set integer overflow flag BTFSS FPFLAGS,SAT ; test for saturation RETLW 0xFF ; return error code in WREG CLRF AARGB0 ; saturate to largest two's BTFSS SIGN,MSB ; complement 24 bit integer MOVLW 0xFF MOVWF AARGB0 ; SIGN = 0, 0x 7F FF FF MOVWF AARGB1 ; SIGN = 1, 0x 80 00 00 MOVWF AARGB2 RLF SIGN,F RRF AARGB0,F RETLW 0xFF ; return error code in WREG ;********************************************************************************************** ;********************************************************************************************** ; Floating Point Multiply ; Input: 24 bit floating point number in AEXP, AARGB0, AARGB1 ; 24 bit floating point number in BEXP, BARGB0, BARGB1 ; Use: CALL FPM24 ; Output: 24 bit floating point product in AEXP, AARGB0, AARGB1 ; Result: AARG <-- AARG * BARG ; Max Timing: 25+15*16+15+18 = 298 clks RND = 0 ; 25+15*16+15+29 = 309 clks RND = 1, SAT = 0 ; 25+15*16+15+33 = 313 clks RND = 1, SAT = 1 ; Min Timing: 6+5 = 11 clks AARG * BARG = 0 ; 24+15*11+14+15 = 218 clks ; PM: 80 DM: 11 ;---------------------------------------------------------------------------------------------- FPM24 MOVF AEXP,W ; test for zero arguments BTFSS STATUS,Z MOVF BEXP,W BTFSC STATUS,Z GOTO RES024 M24BNE0 MOVF AARGB0,W XORWF BARGB0,W MOVWF SIGN ; save sign in SIGN MOVF BEXP,W ADDWF EXP,F MOVLW EXPBIAS-1 BTFSS STATUS,C GOTO MTUN24 SUBWF EXP,F BTFSC STATUS,C GOTO SETFOV24 ; set multiply overflow flag GOTO MOK24 MTUN24 SUBWF EXP,F BTFSS STATUS,C GOTO SETFUN24 MOK24 MOVF AARGB0,W MOVWF AARGB2 ; move result to AARG MOVF AARGB1,W MOVWF AARGB3 BSF AARGB2,MSB ; make argument MSB's explicit BSF BARGB0,MSB BCF STATUS,C CLRF AARGB0 ; clear initial partial product CLRF AARGB1 MOVLW D'16' MOVWF TEMP ; initialize counter MLOOP24 BTFSS AARGB3,LSB ; test next bit GOTO MNOADD24 MADD24 MOVF BARGB1,W ADDWF AARGB1,F MOVF BARGB0,W BTFSC STATUS,C INCFSZ BARGB0,W ADDWF AARGB0,F MNOADD24 RRF AARGB0,F RRF AARGB1,F RRF AARGB2,F RRF AARGB3,F BCF STATUS,C DECFSZ TEMP,F GOTO MLOOP24 BTFSC AARGB0,MSB ; check for postnormalization GOTO MROUND24 RLF AARGB2,F RLF AARGB1,F RLF AARGB0,F DECF EXP,F MROUND24 BTFSC FPFLAGS,RND BTFSS AARGB1,LSB GOTO MUL24OK BTFSS AARGB2,MSB ; round if next bit is set GOTO MUL24OK INCF AARGB1,F BTFSC STATUS,Z INCF AARGB0,F BTFSS STATUS,Z ; has rounding caused carryout? GOTO MUL24OK RRF AARGB0,F ; if so, right shift RRF AARGB1,F INCF EXP,F BTFSC STATUS,Z ; check for overflow GOTO SETFOV24 MUL24OK BTFSS SIGN,MSB BCF AARGB0,MSB ; clear explicit MSB if positive RETLW 0 SETFOV24 BSF FPFLAGS,FOV ; set floating point underflag BTFSS FPFLAGS,SAT ; test for saturation RETLW 0xFF ; return error code in WREG MOVLW 0xFF MOVWF AEXP ; saturate to largest floating MOVWF AARGB0 ; point number = 0x FF 7F FF MOVWF AARGB1 ; modulo the appropriate sign bit RLF SIGN,F RRF AARGB0,F RETLW 0xFF ; return error code in WREG ;********************************************************************************************** ;********************************************************************************************** ; Floating Point Divide ; Input: 24 bit floating point dividend in AEXP, AARGB0, AARGB1 ; 24 bit floating point divisor in BEXP, BARGB0, BARGB1 ; Use: CALL FPD24 ; Output: 24 bit floating point quotient in AEXP, AARGB0, AARGB1 ; Result: AARG <-- AARG / BARG ; Max Timing: 32+13+15*26+25+12 = 472 clks RND = 0 ; 32+13+15*26+25+34 = 494 clks RND = 1, SAT = 0 ; 32+13+15*26+25+38 = 498 clks RND = 1, SAT = 1 ; Min Timing: 7+5 = 12 clks ; PM: 120 DM: 11 ;---------------------------------------------------------------------------------------------- FPD24 MOVF BEXP,W ; test for divide by zero BTFSC STATUS,Z GOTO SETFDZ24 MOVF AEXP,W BTFSC STATUS,Z GOTO RES024 D24BNE0 MOVF AARGB0,W XORWF BARGB0,W MOVWF SIGN ; save sign in SIGN BSF AARGB0,MSB ; make argument MSB's explicit BSF BARGB0,MSB TALIGN24 CLRF TEMP ; clear align increment MOVF AARGB0,W MOVWF AARGB2 ; test for alignment MOVF AARGB1,W MOVWF AARGB3 MOVF BARGB1,W SUBWF AARGB3, f MOVF BARGB0,W BTFSS STATUS,C INCFSZ BARGB0,W SUBWF AARGB2, f CLRF AARGB2 CLRF AARGB3 BTFSS STATUS,C GOTO DALIGN24OK BCF STATUS,C ; align if necessary RRF AARGB0,F RRF AARGB1,F RRF AARGB2,F MOVLW 0x01 MOVWF TEMP ; save align increment DALIGN24OK MOVF BEXP,W ; compare AEXP and BEXP SUBWF EXP,F BTFSS STATUS,C GOTO ALTB24 AGEB24 MOVLW EXPBIAS-1 ADDWF TEMP,W ADDWF EXP,F BTFSC STATUS,C GOTO SETFOV24 GOTO DARGOK24 ; set overflow flag ALTB24 MOVLW EXPBIAS-1 ADDWF TEMP,W ADDWF EXP,F BTFSS STATUS,C GOTO SETFUN24 ; set underflow flag DARGOK24 MOVLW D'16' ; initialize counter MOVWF TEMPB1 DLOOP24 RLF AARGB3,F ; left shift RLF AARGB2,F RLF AARGB1,F RLF AARGB0,F RLF TEMP,F MOVF BARGB1,W ; subtract SUBWF AARGB1,F MOVF BARGB0,W BTFSS STATUS,C INCFSZ BARGB0,W SUBWF AARGB0,F RLF BARGB0,W IORWF TEMP,F BTFSS TEMP,LSB ; test for restore GOTO DREST24 BSF AARGB3,LSB GOTO DOK24 DREST24 MOVF BARGB1,W ; restore if necessary ADDWF AARGB1,F MOVF BARGB0,W BTFSC STATUS,C INCF BARGB0,W ADDWF AARGB0,F BCF AARGB3,LSB DOK24 DECFSZ TEMPB1,F GOTO DLOOP24 DROUND24 BTFSC FPFLAGS,RND BTFSS AARGB3,LSB GOTO DIV24OK BCF STATUS,C RLF AARGB1,F ; compute next significant bit RLF AARGB0,F ; for rounding RLF TEMP,F MOVF BARGB1,W ; subtract SUBWF AARGB1,F MOVF BARGB0,W BTFSS STATUS,C INCFSZ BARGB0,W SUBWF AARGB0,F RLF BARGB0,W IORWF TEMP,W ANDLW 0x01 ADDWF AARGB3,F BTFSC STATUS,C INCF AARGB2,F BTFSS STATUS,Z ; test if rounding caused carryout GOTO DIV24OK RRF AARGB2,F RRF AARGB3,F INCF EXP,F BTFSC STATUS,Z ; test for overflow GOTO SETFOV24 DIV24OK BTFSS SIGN,MSB BCF AARGB2,MSB ; clear explicit MSB if positive MOVF AARGB2,W MOVWF AARGB0 ; move result to AARG MOVF AARGB3,W MOVWF AARGB1 RETLW 0 SETFUN24 BSF FPFLAGS,FUN ; set floating point underflag BTFSS FPFLAGS,SAT ; test for saturation RETLW 0xFF ; return error code in WREG MOVLW 0x01 ; saturate to smallest floating MOVWF AEXP ; point number = 0x 01 00 00 CLRF AARGB0 ; modulo the appropriate sign bit CLRF AARGB1 RLF SIGN,F RRF AARGB0,F RETLW 0xFF ; return error code in WREG SETFDZ24 BSF FPFLAGS,FDZ ; set divide by zero flag RETLW 0xFF ;********************************************************************************************** ;********************************************************************************************** ; Floating Point Subtract ; Input: 24 bit floating point number in AEXP, AARGB0, AARGB1 ; 24 bit floating point number in BEXP, BARGB0, BARGB1 ; Use: CALL FPS24 ; Output: 24 bit floating point sum in AEXP, AARGB0, AARGB1 ; Result: AARG <-- AARG - BARG ; Max Timing: 2+197 = 199 clks RND = 0 ; 2+208 = 210 clks RND = 1, SAT = 0 ; 2+213 = 215 clks RND = 1, SAT = 1 ; Min Timing: 2+12 = 14 clks ; PM: 2+112 = 114 DM: 11 ;---------------------------------------------------------------------------------------------- FPS24 MOVLW 0x80 XORWF BARGB0,F ;********************************************************************************************** ; Floating Point Add ; Input: 24 bit floating point number in AEXP, AARGB0, AARGB1 ; 24 bit floating point number in BEXP, BARGB0, BARGB1 ; Use: CALL FPA24 ; Output: 24 bit floating point sum in AEXP, AARGB0, AARGB1 ; Result: AARG <-- AARG - BARG ; Max Timing: 25+28+6*6+5+31+72 = 197 clks RND = 0 ; 25+28+6*6+5+42+72 = 208 clks RND = 1, SAT = 0 ; 25+28+6*6+5+42+77 = 213 clks RND = 1, SAT = 1 ; Min Timing: 8+4 = 12 clks ; PM: 112 DM: 11 ;---------------------------------------------------------------------------------------------- FPA24 MOVF AARGB0,W ; exclusive or of signs in TEMP XORWF BARGB0,W MOVWF TEMP CLRF AARGB2 ; clear extended byte CLRF BARGB2 MOVF AEXP,W ; use AARG if AEXP >= BEXP SUBWF BEXP,W BTFSS STATUS,C GOTO USEA24 MOVF BEXP,W ; use BARG if AEXP < BEXP MOVWF AARGB4 ; therefore, swap AARG and BARG MOVF AEXP,W MOVWF BEXP MOVF AARGB4,W MOVWF AEXP MOVF BARGB0,W MOVWF AARGB4 MOVF AARGB0,W MOVWF BARGB0 MOVF AARGB4,W MOVWF AARGB0 MOVF BARGB1,W MOVWF AARGB4 MOVF AARGB1,W MOVWF BARGB1 MOVF AARGB4,W MOVWF AARGB1 USEA24 MOVF BEXP,W ; return AARG if BARG = 0 BTFSC STATUS,Z RETLW 0x00 MOVF AARGB0,W MOVWF SIGN ; save sign in SIGN BSF AARGB0,MSB ; make MSB's explicit BSF BARGB0,MSB MOVF BEXP,W ; compute shift count in BEXP SUBWF AEXP,W MOVWF BEXP BTFSC STATUS,Z GOTO ALIGNED24 MOVLW 8 SUBWF BEXP,W BTFSS STATUS,C ; if BEXP >= 8, do byte shift GOTO ALIGNB24 MOVWF BEXP MOVF BARGB1,W ; keep for postnormalization MOVWF BARGB2 MOVF BARGB0,W MOVWF BARGB1 CLRF BARGB0 MOVLW 8 SUBWF BEXP,W BTFSS STATUS,C ; if BEXP >= 8, BARG = 0 relative to AARG GOTO ALIGNB24 MOVF SIGN,W MOVWF AARGB0 RETLW 0x00 ALIGNB24 MOVF BEXP,W ; already aligned if BEXP = 0 BTFSC STATUS,Z GOTO ALIGNED24 ALOOPB24 BCF STATUS,C ; right shift by BEXP RRF BARGB0,F RRF BARGB1,F RRF BARGB2,F DECFSZ BEXP,F GOTO ALOOPB24 ALIGNED24 BTFSS TEMP,MSB ; negate if signs opposite GOTO AOK24 COMF BARGB2,F COMF BARGB1,F COMF BARGB0,F INCF BARGB2,F BTFSC STATUS,Z INCF BARGB1,F BTFSC STATUS,Z INCF BARGB0,F AOK24 MOVF BARGB2,W ADDWF AARGB2,F MOVF BARGB1,W BTFSC STATUS,C INCFSZ BARGB1,W ADDWF AARGB1,F MOVF BARGB0,W BTFSC STATUS,C INCFSZ BARGB0,W ADDWF AARGB0,F BTFSC TEMP,MSB GOTO ACOMP24 BTFSS STATUS,C GOTO NRMRND3224 RRF AARGB0,F ; shift right and increment EXP RRF AARGB1,F RRF AARGB2,F INCFSZ AEXP,F GOTO NRMRND3224 GOTO SETFOV24 ACOMP24 BTFSC STATUS,C GOTO NRM3224 ; normalize and fix sign COMF AARGB2,F COMF AARGB1,F ; negate, toggle sign bit and COMF AARGB0,F ; then normalize INCF AARGB2,F BTFSC STATUS,Z INCF AARGB1,F BTFSC STATUS,Z INCF AARGB0,F MOVLW 0x80 XORWF SIGN,F GOTO NRM3224 END ;******************************************************************** UID=0 GID=0 EUID=0 EGID=0