; CC8E Version 1.3, Copyright (c) B Knudsen Data ; C compiler for the PIC18 microcontrollers ; ************ 24. Jun 2009 9:08 ************* processor PIC18C252 radix DEC PRODH EQU 0xFF4 PRODL EQU 0xFF3 Carry EQU 0 Zero_ EQU 2 FpFlags EQU 0x0A FpOverflow EQU 1 FpUnderFlow EQU 2 FpDiv0 EQU 3 FpRounding EQU 6 arg1f24 EQU 0x00 arg2f24 EQU 0x03 aarg EQU 0x06 sign EQU 0x08 tmpL EQU 0x09 aarg_2 EQU 0x06 sign_2 EQU 0x08 counter EQU 0x09 xtra EQU 0x06 temp EQU 0x07 expo EQU 0x08 sign_3 EQU 0x09 expo_2 EQU 0x06 xtra_2 EQU 0x07 sign_4 EQU 0x08 rval EQU 0x00 sign_6 EQU 0x06 expo_4 EQU 0x07 xtra_4 EQU 0x08 rval_3 EQU 0x00 ad_data EQU 0x0B tx EQU 0x0D av EQU 0x10 mg EQU 0x13 a EQU 0x16 vx EQU 0x19 prev EQU 0x1C kp EQU 0x1F GOTO main ; FILE 18\math24f.h ;// ************************************************* ;// 24 bit basic floating point math operations ;// Copyright (c) B Knudsen Data, Norway, 2000 - 2005 ;// ************************************************* ; ;#pragma library 1 ;/* PROTOTYPES for page definition in application header file: ;float24 operator* _fmul24( float24 arg1f24, float24 arg2f24); ;float24 operator/ _fdiv24( float24 arg1f24, float24 arg2f24); ;float24 operator+ _fadd24( float24 arg1f24, float24 arg2f24); ;float24 operator- _fsub24( float24 arg1f24, float24 arg2f24); ;float24 operator= _int24ToFloat24( int24 arg1f24); ;float24 operator= _int32ToFloat24( int32 arg32); ;int24 operator= _float24ToInt24( float24 arg1f24); ;bit operator< _f24_LT_f24( float24 arg1f24, float24 arg2f24); ;bit operator>= _f24_GE_f24( float24 arg1f24, float24 arg2f24); ;bit operator> _f24_GT_f24( float24 arg1f24, float24 arg2f24); ;bit operator<= _f24_LE_f24( float24 arg1f24, float24 arg2f24); ;*/ ; ;// DEFINABLE SYMBOLS (in the application code): ;//#define FP_OPTIM_SPEED // optimize for SPEED: default ;//#define FP_OPTIM_SIZE // optimize for SIZE ;//#define DISABLE_ROUNDING // disable rounding and save code space ; ;#define float24ToIEEE754(a) { a.mid8=rl(a.mid8); a.high8=rr(a.high8);\ ; a.mid8=rr(a.mid8); } ;#define IEEE754ToFloat24(a) { a.mid8=rl(a.mid8); a.high8=rl(a.high8);\ ; a.mid8=rr(a.mid8); } ; ; ;/* 24 bit floating point format: ; ; address ID ; X a.low8 : LSB, bit 0-7 of mantissa ; X+1 a.mid8 : bit 8-14 of mantissa, bit 15 is the sign bit ; X+2 a.high8 : MSB, bit 0-7 of exponent, with bias 0x7F ; ; bit 15 of mantissa is a hidden bit, always equal to 1 ; zero (0.0) : a.high8 = 0 (mantissa & sign ignored) ; ; MSB LSB ; 7F 00 00 : 1.0 = 1.0 * 2**(0x7F-0x7F) = 1.0 * 1 ; 7F 80 00 : -1.0 = -1.0 * 2**(0x7F-0x7F) = -1.0 * 1 ; 80 00 00 : 2.0 = 1.0 * 2**(0x80-0x7F) = 1.0 * 2 ; 80 40 00 : 3.0 = 1.5 * 2**(0x80-0x7F) = 1.5 * 2 ; 7E 60 00 : 0.875 = 1.75 * 2**(0x7E-0x7F) = 1.75 * 0.5 ; 7F 60 00 : 1.75 = 1.75 * 2**(0x7E-0x7F) = 1.75 * 1 ; 7F 7F FF : 1.999969482 ; 00 7C 5A : 0.0 (mantissa & sign ignored) ; 01 00 00 : 1.17549435e-38 = 1.0 * 2**(0x01-0x7F) ; FE 7F FF : 3.40277175e+38 = 1.999969482 * 2**(0xFE-0x7F) ; FF 00 00 : +INF : positive infinity ; FF 80 00 : -INF : negative infinity ;*/ ; ;#define FpBIAS 0x7F ; ;#ifndef FpFlags_defined ; #define FpFlags_defined ; ; char FpFlags; ; //bit IOV @ FpFlags.0; // integer overflow flag: NOT USED ; bit FpOverflow @ FpFlags.1; // floating point overflow flag ; bit FpUnderFlow @ FpFlags.2; // floating point underflow flag ; bit FpDiv0 @ FpFlags.3; // floating point divide by zero flag ; //bit FpNAN @ FpFlags.4; // not-a-number exception flag: NOT USED ; bit FpDomainError @ FpFlags.5; // domain error exception flag ; bit FpRounding @ FpFlags.6; // floating point rounding flag, 0=truncation ; // 1 = unbiased rounding to nearest LSB ; //bit FpSaturate @ FpFlags.7; // floating point saturate flag: NOT USED ; ; #pragma floatOverflow FpOverflow ; #pragma floatUnderflow FpUnderFlow ; ; #define InitFpFlags() FpFlags = 0x40 /* enable rounding as default */ ;#endif ; ;#ifdef DISABLE_ROUNDING ; #pragma floatRounding 0 ;#endif ; ; ;#if __CoreSet__ < 1600 ; #define genAdd(r,a) W=a; btsc(Carry); W=incsz(a); r+=W ; #define genSub(r,a) W=a; btss(Carry); W=incsz(a); r-=W ; #define genAddW(r,a) W=a; btsc(Carry); W=incsz(a); W=r+W ; #define genSubW(r,a) W=a; btss(Carry); W=incsz(a); W=r-W ;#else ; #define genAdd(r,a) W=a; r=addWFC(r) ; #define genSub(r,a) W=a; r=subWFB(r) ; #define genAddW(r,a) W=a; W=addWFC(r) ; #define genSubW(r,a) W=a; W=subWFB(r) ;#endif ; ;#if __CoreSet__ == 1700 || __CoreSet__ == 1800 ; #define hw_mult8x8(a,b) W = a; multiply(b) ; #define loRES PRODL ; #define hiRES PRODH ;#endif ; ;#if __CoreSet__ == 2000 ; #define hw_mult8x8(a,b) W = a; multiply(b) ; #define loRES W ; #define hiRES MULH ;#endif ; ; ;float24 operator* _fmul24( sharedM float24 arg1f24, sharedM float24 arg2f24) ;{ _fmul24 ; uns16 aarg; ; W = arg1f24.mid8; MOVF arg1f24+1,W,0 ; aarg.high8 = W; MOVWF aarg+1,0 ; ; // save sign ; char sign = arg2f24.mid8 ^ W; // before first overflow test XORWF arg2f24+1,W,0 MOVWF sign,0 ; ; W = arg1f24.high8; MOVF arg1f24+2,W,0 ; #if __CoreSet__ / 100 == 17 ; if (W != 0) ; W = arg2f24.high8; ; if (W == 0) ; goto RES0; ; #else ; if (!Zero_) BTFSS 0xFD8,Zero_,0 ; W = arg2f24.high8; MOVF arg2f24+2,W,0 ; if (Zero_) ; goto RES0; BZ m005 ; #endif ; ; arg1f24.high8 += W /* arg2f24.high8 */; ADDWF arg1f24+2,1,0 ; W = FpBIAS-1; MOVLW 126 ; if (Carry) { BNC m001 ; arg1f24.high8 -= W; SUBWF arg1f24+2,1,0 ; if (Carry) BNC m002 ; goto OVERFLOW; BRA m006 ; } ; else { ; arg1f24.high8 -= W; m001 SUBWF arg1f24+2,1,0 ; if (!Carry) ; goto UNDERFLOW; BNC m004 ; } ; aarg.low8 = arg1f24.low8; m002 MOVFF arg1f24,aarg ; ; aarg.15 = 1; BSF aarg+1,7,0 ; arg2f24.15 = 1; BSF arg2f24+1,7,0 ; ; #if defined hw_mult8x8 && !defined FP_OPTIM_SIZE ; ; char tmpL; ; arg1f24.low16 = 0; CLRF arg1f24,0 CLRF arg1f24+1,0 ; ; hw_mult8x8( arg2f24.low8, aarg.low8); // p1 MOVF arg2f24,W,0 MULWF aarg,0 ; tmpL = hiRES; MOVFF PRODH,tmpL ; ; hw_mult8x8( arg2f24.mid8, aarg.low8); // p2 MOVF arg2f24+1,W,0 MULWF aarg,0 ; tmpL += loRES; MOVF PRODL,W,0 ADDWF tmpL,1,0 ; genAdd( arg1f24.low8, hiRES); MOVF PRODH,W,0 ADDWFC arg1f24,1,0 ; genAdd( arg1f24.mid8, 0); MOVLW 0 ADDWFC arg1f24+1,1,0 ; ; hw_mult8x8( arg2f24.low8, aarg.mid8); // p2 MOVF arg2f24,W,0 MULWF aarg+1,0 ; tmpL += loRES; MOVF PRODL,W,0 ADDWF tmpL,1,0 ; genAdd( arg1f24.low8, hiRES); MOVF PRODH,W,0 ADDWFC arg1f24,1,0 ; genAdd( arg1f24.mid8, 0); MOVLW 0 ADDWFC arg1f24+1,1,0 ; ; hw_mult8x8( arg2f24.mid8, aarg.mid8); // p3 MOVF arg2f24+1,W,0 MULWF aarg+1,0 ; arg1f24.low8 += loRES; MOVF PRODL,W,0 ADDWF arg1f24,1,0 ; genAdd( arg1f24.mid8, hiRES); MOVF PRODH,W,0 ADDWFC arg1f24+1,1,0 ; ; #undef hw_mult8x8 ; #undef loRES ; #undef hiRES ; ; if (!arg1f24.15) { BTFSC arg1f24+1,7,0 BRA m003 ; tmpL = rl( tmpL); RLCF tmpL,1,0 ; arg1f24.low16 = rl( arg1f24.low16); RLCF arg1f24,1,0 RLCF arg1f24+1,1,0 ; if (arg1f24.high8 == 0) MOVF arg1f24+2,1,0 ; goto UNDERFLOW; BZ m004 ; arg1f24.high8 -= 1; DECF arg1f24+2,1,0 ; W = rl( tmpL); // restore bit behind LSB in Carry RLCF tmpL,W,0 ; } ; ; #else ; ; arg1f24.low16 = 0; ; ; char counter = sizeof(aarg)*8; ; ; do { ; aarg = rr( aarg); ; if (Carry) { ; arg1f24.low8 += arg2f24.low8; ; genAdd( arg1f24.mid8, arg2f24.mid8); ; } ; arg1f24.low16 = rr( arg1f24.low16); ; counter = decsz(counter); ; } while (1); ; ; if (!arg1f24.15) { ; // catch Carry bit that was shifted out previously ; arg1f24.low16 = rl( arg1f24.low16); ; if (arg1f24.high8 == 0) ; goto UNDERFLOW; ; arg1f24.high8 -= 1; ; W = rl( aarg.high8); ; // restore bit behind LSB in Carry ; } ; ; #endif ; ; #ifndef DISABLE_ROUNDING ; if (FpRounding && Carry) { m003 BTFSS 0x0A,FpRounding,0 BRA m008 BNC m008 ; arg1f24.low8 += 1; INCFSZ arg1f24,1,0 ; if (!arg1f24.low8) { BRA m008 ; arg1f24.mid8 += 1; INCFSZ arg1f24+1,1,0 ; if (!arg1f24.mid8) { BRA m008 ; #if __CoreSet__ >= 1700 && __CoreSet__ <= 1800 ; Carry = 1; // previous INCF changes Carry BSF 0xFD8,Carry,0 ; #else ; // Carry = 1; //OK ; #endif ; arg1f24.low16 = rr( arg1f24.low16); RRCF arg1f24+1,1,0 RRCF arg1f24,1,0 ; arg1f24.high8 += 1; INCFSZ arg1f24+2,1,0 ; if (Zero_) BRA m008 ; goto OVERFLOW; BRA m006 ; } ; } ; } ; #endif ; goto SET_SIGN; ; ; UNDERFLOW: ; FpUnderFlow = 1; m004 BSF 0x0A,FpUnderFlow,0 ; RES0: ; arg1f24.high8 = 0; m005 CLRF arg1f24+2,0 ; goto MANTISSA; BRA m007 ; ; OVERFLOW: ; FpOverflow = 1; m006 BSF 0x0A,FpOverflow,0 ; arg1f24.high8 = 0xFF; SETF arg1f24+2,0 ; MANTISSA: ; arg1f24.low16 = 0x8000; m007 CLRF arg1f24,0 MOVLW 128 MOVWF arg1f24+1,0 ; ; SET_SIGN: ; if (!(sign & 0x80)) m008 BTFSS sign,7,0 ; arg1f24.15 = 0; BCF arg1f24+1,7,0 ; return arg1f24; MOVF arg1f24,W,0 RETURN ;} ; ; ; ;float24 operator/ _fdiv24( sharedM float24 arg1f24, sharedM float24 arg2f24) ;{ _fdiv24 ; uns16 aarg; ; W = arg1f24.mid8; MOVF arg1f24+1,W,0 ; aarg.high8 = W; MOVWF aarg_2+1,0 ; ; // save sign ; char sign = arg2f24.mid8 ^ W; // before first overflow test XORWF arg2f24+1,W,0 MOVWF sign_2,0 ; ; #if __CoreSet__ / 100 == 17 ; if (!arg2f24.high8) ; goto Div0; ; #else ; W = arg2f24.high8; MOVF arg2f24+2,W,0 ; if (Zero_) ; goto Div0; BZ m019 ; #endif ; if (!arg1f24.high8) MOVF arg1f24+2,1,0 ; goto RES0; BZ m021 ; ; arg1f24.high8 -= arg2f24.high8; SUBWF arg1f24+2,1,0 ; W = FpBIAS; MOVLW 127 ; if (!Carry) { BC m009 ; arg1f24.high8 += W; ADDWF arg1f24+2,1,0 ; if (!Carry) BC m010 ; goto UNDERFLOW; BRA m020 ; } ; else { ; arg1f24.high8 += W; m009 ADDWF arg1f24+2,1,0 ; if (Carry) ; goto OVERFLOW; BC m022 ; } ; ; aarg.low8 = arg1f24.low8; m010 MOVFF arg1f24,aarg_2 ; aarg.15 = 1; BSF aarg_2+1,7,0 ; arg2f24.15 = 1; BSF arg2f24+1,7,0 ; ; // division: shift & add ; char counter = 16; MOVLW 16 MOVWF counter,0 ; arg1f24.low16 = 0; // speedup CLRF arg1f24,0 CLRF arg1f24+1,0 ; ;#if defined FP_OPTIM_SPEED || !defined FP_OPTIM_SIZE // SPEED ; ; goto START; BRA m013 ; ; TEST_ZERO_L: ; W = aarg.low8 - arg2f24.low8; m011 MOVF arg2f24,W,0 SUBWF aarg_2,W,0 ; if (!Carry) ; goto SHIFT_IN_CARRY; BNC m016 ; aarg.low8 = W; MOVWF aarg_2,0 ; aarg.high8 = 0; CLRF aarg_2+1,0 ; goto SET_AND_SHIFT_IN_CARRY; BRA m015 ; ;// MAIN LOOP ; do { ; LOOP: ; if (!Carry) { m012 BC m014 ; START: ; W = aarg.high8 - arg2f24.mid8; m013 MOVF arg2f24+1,W,0 SUBWF aarg_2+1,W,0 ; if (Zero_) ; goto TEST_ZERO_L; BZ m011 ; if (!Carry) ; goto SHIFT_IN_CARRY; BNC m016 ; } ; aarg.low8 -= arg2f24.low8; m014 MOVF arg2f24,W,0 SUBWF aarg_2,1,0 ; genSub( aarg.high8, arg2f24.mid8); MOVF arg2f24+1,W,0 SUBWFB aarg_2+1,1,0 ; SET_AND_SHIFT_IN_CARRY: ; Carry = 1; m015 BSF 0xFD8,Carry,0 ; SHIFT_IN_CARRY: ; arg1f24.low16 = rl( arg1f24.low16); m016 RLCF arg1f24,1,0 RLCF arg1f24+1,1,0 ; // Carry = 0; // ok, speedup ; aarg = rl( aarg); RLCF aarg_2,1,0 RLCF aarg_2+1,1,0 ; counter = decsz(counter); DECFSZ counter,1,0 ; } while (1); BRA m012 ; ; ; ;#else // SIZE ; ; goto START; ; ;// MAIN LOOP ; do { ; LOOP: ; if (Carry) ; goto SUBTRACT; ; START: ; W = aarg.low8 - arg2f24.low8; ; genSubW( aarg.high8, arg2f24.mid8); ; if (!Carry) ; goto SKIP_SUB; ; SUBTRACT: ; aarg.low8 -= arg2f24.low8; ; genSub( aarg.high8, arg2f24.mid8); ; Carry = 1; ; SKIP_SUB: ; arg1f24.low16 = rl( arg1f24.low16); ; // Carry = 0; // ok ; aarg = rl( aarg); ; counter = decsz(counter); ; } while (1); ; ;#endif ; ; if (!arg1f24.15) { BTFSC arg1f24+1,7,0 BRA m017 ; if (!arg1f24.high8) MOVF arg1f24+2,1,0 ; goto UNDERFLOW; BZ m020 ; #if __CoreSet__ >= 1700 && __CoreSet__ <= 1800 ; sign = rr( sign); // Save Carry RRCF sign_2,1,0 ; #endif ; arg1f24.high8 --; DECF arg1f24+2,1,0 ; counter ++; INCF counter,1,0 ; #if __CoreSet__ >= 1700 && __CoreSet__ <= 1800 ; sign = rl( sign); // Restore Carry, changed by INCF/DECF RLCF sign_2,1,0 ; #endif ; goto LOOP; BRA m012 ; } ; ; #ifndef DISABLE_ROUNDING ; if (FpRounding) { m017 BTFSS 0x0A,FpRounding,0 BRA m025 ; if (Carry) ; goto ADD_1; BC m018 ; aarg.low8 -= arg2f24.low8; MOVF arg2f24,W,0 SUBWF aarg_2,1,0 ; genSub( aarg.high8, arg2f24.mid8); MOVF arg2f24+1,W,0 SUBWFB aarg_2+1,1,0 ; if (Carry) { BNC m025 ; ADD_1: ; arg1f24.low8 += 1; m018 INCFSZ arg1f24,1,0 ; if (!arg1f24.low8) { BRA m025 ; arg1f24.mid8 ++; INCFSZ arg1f24+1,1,0 ; if (!arg1f24.mid8) { BRA m025 ; arg1f24.low16 = rr( arg1f24.low16); RRCF arg1f24+1,1,0 RRCF arg1f24,1,0 ; arg1f24.high8 ++; INCFSZ arg1f24+2,1,0 ; if (!arg1f24.high8) BRA m025 ; goto OVERFLOW; BRA m022 ; } ; } ; } ; } ; #endif ; goto SET_SIGN; ; ; Div0: ; FpDiv0 = 1; m019 BSF 0x0A,FpDiv0,0 ; goto SATURATE; BRA m023 ; ; UNDERFLOW: ; FpUnderFlow = 1; m020 BSF 0x0A,FpUnderFlow,0 ; RES0: ; arg1f24.high8 = 0; m021 CLRF arg1f24+2,0 ; goto MANTISSA; BRA m024 ; ; OVERFLOW: ; FpOverflow = 1; m022 BSF 0x0A,FpOverflow,0 ; SATURATE: ; arg1f24.high8 = 0xFF; m023 SETF arg1f24+2,0 ; MANTISSA: ; arg1f24.low16 = 0x8000; m024 CLRF arg1f24,0 MOVLW 128 MOVWF arg1f24+1,0 ; ; SET_SIGN: ; if (!(sign & 0x80)) m025 BTFSS sign_2,7,0 ; arg1f24.15 = 0; BCF arg1f24+1,7,0 ; return arg1f24; MOVF arg1f24,W,0 RETURN ;} ; ; ;float24 operator+ _fadd24( sharedM float24 arg1f24, sharedM float24 arg2f24) ;{ _fadd24 ; char xtra, temp; ; char expo = arg1f24.high8 - arg2f24.high8; MOVF arg2f24+2,W,0 SUBWF arg1f24+2,W,0 MOVWF expo,0 ; if (!Carry) { BC m026 ; expo = -expo; NEGF expo,0 ; temp = arg1f24.high8; MOVFF arg1f24+2,temp ; arg1f24.high8 = arg2f24.high8; MOVFF arg2f24+2,arg1f24+2 ; arg2f24.high8 = temp; MOVFF temp,arg2f24+2 ; temp = arg1f24.mid8; MOVFF arg1f24+1,temp ; arg1f24.mid8 = arg2f24.mid8; MOVFF arg2f24+1,arg1f24+1 ; arg2f24.mid8 = temp; MOVFF temp,arg2f24+1 ; temp = arg1f24.low8; MOVFF arg1f24,temp ; arg1f24.low8 = arg2f24.low8; MOVFF arg2f24,arg1f24 ; arg2f24.low8 = temp; MOVFF temp,arg2f24 ; } ; if (expo > sizeof(arg1f24)*8-7) m026 MOVLW 18 CPFSLT expo,0 ; goto RETURN; BRA m044 ; if (!arg2f24.high8) MOVF arg2f24+2,1,0 ; goto RETURN; // result is arg1f24 BZ m044 ; ; xtra = 0; CLRF xtra,0 ; ; W = arg1f24.mid8; MOVF arg1f24+1,W,0 ; temp = W; MOVWF temp,0 ; char sign = arg2f24.mid8 ^ W; XORWF arg2f24+1,W,0 MOVWF sign_3,0 ; arg1f24.15 = 1; BSF arg1f24+1,7,0 ; arg2f24.15 = 1; BSF arg2f24+1,7,0 ; ; while (1) { ; W = 8; m027 MOVLW 8 ; expo -= W; SUBWF expo,1,0 ; if (!Carry) ; break; BNC m028 ; xtra = arg2f24.low8; MOVFF arg2f24,xtra ; arg2f24.low8 = arg2f24.mid8; MOVFF arg2f24+1,arg2f24 ; arg2f24.mid8 = 0; CLRF arg2f24+1,0 ; } BRA m027 ; expo += W; m028 ADDWF expo,1,0 ; if (expo) { BZ m030 ; do { ; Carry = 0; m029 BCF 0xFD8,Carry,0 ; arg2f24.low16 = rr( arg2f24.low16); RRCF arg2f24+1,1,0 RRCF arg2f24,1,0 ; xtra = rr( xtra); RRCF xtra,1,0 ; } while (--expo > 0); DECFSZ expo,1,0 BRA m029 ; } ; ; ; if (sign & 0x80) { m030 BTFSS sign_3,7,0 BRA m036 ; // SUBTRACT ; arg1f24.low8 -= arg2f24.low8; MOVF arg2f24,W,0 SUBWF arg1f24,1,0 ; genSub( arg1f24.mid8, arg2f24.mid8); MOVF arg2f24+1,W,0 SUBWFB arg1f24+1,1,0 ; if (!Carry) { // arg2f24 > arg1f24 BC m031 ; arg1f24.low16 = -arg1f24.low16; BSF 0xFD8,Carry,0 MOVLW 0 SUBFWB arg1f24,1,0 SUBFWB arg1f24+1,1,0 ; // xtra == 0 because arg1f24.exp == arg2f24.exp ; temp ^= 0x80; // invert sign MOVLW 128 XORWF temp,1,0 ; } ; xtra = -xtra; m031 NEGF xtra,0 ; if (xtra) BZ m032 ; arg1f24.low16 --; DECF arg1f24,1,0 MOVLW 0 SUBWFB arg1f24+1,1,0 ; // adjust result left ; #define counter expo ; counter = 3; m032 MOVLW 3 MOVWF expo,0 ; while (!arg1f24.mid8) { m033 TSTFSZ arg1f24+1,0 BRA m034 ; arg1f24.mid8 = arg1f24.low8; MOVFF arg1f24,arg1f24+1 ; arg1f24.low8 = xtra; MOVFF xtra,arg1f24 ; xtra = 0; CLRF xtra,0 ; arg1f24.high8 -= 8; MOVLW 8 SUBWF arg1f24+2,1,0 ; if (!Carry) ; goto RES0; BNC m040 ; if (--counter == 0) // max 2 iterations DECFSZ expo,1,0 BRA m033 ; goto RES0; BRA m040 ; } ; #undef counter ; while (!arg1f24.15) { m034 BTFSC arg1f24+1,7,0 BRA m035 ; Carry = 0; BCF 0xFD8,Carry,0 ; xtra = rl( xtra); RLCF xtra,1,0 ; arg1f24.low16 = rl( arg1f24.low16); RLCF arg1f24,1,0 RLCF arg1f24+1,1,0 ; arg1f24.high8 --; DECFSZ arg1f24+2,1,0 ; if (!arg1f24.high8) BRA m034 ; goto RES0; // UNDERFLOW? BRA m040 ; } ; #ifndef DISABLE_ROUNDING ; if (FpRounding && (xtra & 0x80)) { m035 BTFSS 0x0A,FpRounding,0 BRA m043 BTFSS xtra,7,0 BRA m043 ; xtra = 0; // disable recursion CLRF xtra,0 ; goto INCREMENT; BRA m039 ; } ; #endif ; } ; else { ; // ADD arg1f24 and arg2f24 ; arg1f24.low8 += arg2f24.low8; m036 MOVF arg2f24,W,0 ADDWF arg1f24,1,0 ; genAdd( arg1f24.mid8, arg2f24.mid8); MOVF arg2f24+1,W,0 ADDWFC arg1f24+1,1,0 ; if (Carry) { BNC m038 ; ADJUST_RIGHT: ; arg1f24.low16 = rr( arg1f24.low16); m037 RRCF arg1f24+1,1,0 RRCF arg1f24,1,0 ; xtra = rr( xtra); RRCF xtra,1,0 ; arg1f24.high8 += 1; // exp INFSNZ arg1f24+2,1,0 ; if (!arg1f24.high8) ; goto OVERFLOW; BRA m041 ; } ; #ifndef DISABLE_ROUNDING ; if (FpRounding && (xtra & 0x80)) { m038 BTFSS 0x0A,FpRounding,0 BRA m043 BTFSS xtra,7,0 BRA m043 ; INCREMENT: ; arg1f24.low8 += 1; m039 INCFSZ arg1f24,1,0 ; if (!arg1f24.low8) { BRA m043 ; arg1f24.mid8 += 1; INCFSZ arg1f24+1,1,0 ; if (!arg1f24.mid8) { BRA m043 ; Carry = 1; // prepare for shift BSF 0xFD8,Carry,0 ; arg1f24.0 = 0; // disable recursion BCF arg1f24,0,0 ; goto ADJUST_RIGHT; BRA m037 ; } ; } ; } ; #endif ; } ; goto SET_SIGN; ; ;// UNDERFLOW: ;// FpUnderFlow = 1; ; RES0: ; arg1f24.high8 = 0; m040 CLRF arg1f24+2,0 ; goto MANTISSA; BRA m042 ; ; OVERFLOW: ; FpOverflow = 1; m041 BSF 0x0A,FpOverflow,0 ; arg1f24.high8 = 0xFF; SETF arg1f24+2,0 ; MANTISSA: ; arg1f24.low16 = 0x8000; m042 CLRF arg1f24,0 MOVLW 128 MOVWF arg1f24+1,0 ; ; SET_SIGN: ; if (!(temp & 0x80)) m043 BTFSS temp,7,0 ; arg1f24.15 = 0; BCF arg1f24+1,7,0 ; ; RETURN: ; return arg1f24; m044 MOVF arg1f24,W,0 RETURN ;} ; ; ;// SUBTRACTION ; ;float24 operator- _fsub24( sharedM float24 arg1f24, sharedM float24 arg2f24) ;{ _fsub24 ; arg2f24.mid8 ^= 0x80; ; arg1f24 += arg2f24; ; return arg1f24; ;} ; ; ;float24 operator=( int8 arg) @ ;float24 operator=( uns8 arg) @ ;float24 operator=( int16 arg) @ ;float24 operator=( uns16 arg) @ ;float24 operator= _int24ToFloat24( sharedM int24 arg1f24) ;{ _int24ToFloat24 ; sharedM float24 arg2f24; // unused, but required ; char expo = FpBIAS + 16 - 1; MOVLW 142 MOVWF expo_2,0 ; char xtra = 0; CLRF xtra_2,0 ; char sign = 0; CLRF sign_4,0 ; if (arg1f24 < 0) { BTFSS arg1f24+2,7,0 BRA m045 ; arg1f24 = -arg1f24; BSF 0xFD8,Carry,0 MOVLW 0 SUBFWB arg1f24,1,0 SUBFWB arg1f24+1,1,0 SUBFWB arg1f24+2,1,0 ; sign |= 0x80; BSF sign_4,7,0 ; } ; if (arg1f24.high8) { m045 MOVF arg1f24+2,1,0 BZ m046 ; expo += 8; MOVLW 8 ADDWF expo_2,1,0 ; xtra = arg1f24.low8; MOVFF arg1f24,xtra_2 ; arg1f24.low8 = arg1f24.mid8; MOVFF arg1f24+1,arg1f24 ; arg1f24.mid8 = arg1f24.high8; MOVFF arg1f24+2,arg1f24+1 ; } ; else if (!arg1f24.mid8) { BRA m048 m046 TSTFSZ arg1f24+1,0 BRA m048 ; expo -= 8; MOVLW 8 SUBWF expo_2,1,0 ; W = arg1f24.low8; MOVF arg1f24,W,0 ; if (!W) ; goto RETURN; BZ m050 ; arg1f24.mid8 = W; MOVWF arg1f24+1,0 ; arg1f24.low8 = 0; CLRF arg1f24,0 ; } ; ; // arg1f24.mid8 != 0 ; goto TEST_ARG1_B15; BRA m048 ; do { ; xtra = rl( xtra); m047 RLCF xtra_2,1,0 ; arg1f24.low16 = rl( arg1f24.low16); RLCF arg1f24,1,0 RLCF arg1f24+1,1,0 ; expo --; DECF expo_2,1,0 ; TEST_ARG1_B15: ; } while (!arg1f24.15); m048 BTFSS arg1f24+1,7,0 BRA m047 ; ; #ifndef DISABLE_ROUNDING ; if (FpRounding && (xtra & 0x80)) { BTFSS 0x0A,FpRounding,0 BRA m049 BTFSS xtra_2,7,0 BRA m049 ; arg1f24.low8 += 1; INCFSZ arg1f24,1,0 ; if (!arg1f24.low8) { BRA m049 ; arg1f24.mid8 += 1; INCFSZ arg1f24+1,1,0 ; if (!arg1f24.mid8) { BRA m049 ; Carry = 1; BSF 0xFD8,Carry,0 ; arg1f24.low16 = rr( arg1f24.low16); RRCF arg1f24+1,1,0 RRCF arg1f24,1,0 ; expo ++; INCF expo_2,1,0 ; } ; } ; } ; #endif ; ; arg1f24.high8 = expo; m049 MOVFF expo_2,arg1f24+2 ; if (!(sign & 0x80)) BTFSS sign_4,7,0 ; arg1f24.15 = 0; BCF arg1f24+1,7,0 ; ; RETURN: ; float24 rval @ arg1f24; ; rval.low24 = arg1f24.low24; ; return rval; m050 MOVF rval,W,0 RETURN ;} ; ; ;float24 operator=( uns24 arg) @ ;float24 operator= _int32ToFloat24( int32 arg32) ;{ _int32ToFloat24 ; char expo = FpBIAS + 16 - 1; ; char xtra @ arg32.high8; ; char sign = 0; ; if (arg32 < 0) { ; arg32 = -arg32; ; sign |= 0x80; ; } ; if (arg32.high8) { ; expo += 8; ; arg32.low8 = arg32.midL8; ; arg32.midL8 = arg32.midH8; ; arg32.midH8 = arg32.high8; ; arg32.high8 = 0; ; } ; if (arg32.midH8) { ; expo += 8; ; xtra = arg32.low8; ; arg32.low8 = arg32.midL8; ; arg32.midL8 = arg32.midH8; ; } ; else if (!arg32.midL8) { ; expo -= 8; ; W = arg32.low8; ; if (!W) ; goto RETURN; ; arg32.midL8 = W; ; arg32.low8 = 0; ; } ; ; // arg32.midL8 != 0 ; goto TEST_ARG_B15; ; do { ; xtra = rl( xtra); ; arg32.low16 = rl( arg32.low16); ; expo --; ; TEST_ARG_B15: ; } while (!arg32.15); ; ; #ifndef DISABLE_ROUNDING ; if (FpRounding && (xtra & 0x80)) { ; arg32.low8 += 1; ; if (!arg32.low8) { ; arg32.midL8 += 1; ; if (!arg32.midL8) { ; Carry = 1; ; arg32.low16 = rr( arg32.low16); ; expo ++; ; } ; } ; } ; #endif ; ; arg32.midH8 = expo; ; if (!(sign & 0x80)) ; arg32.15 = 0; ; ; RETURN: ; float24 rval @ arg32; ; rval.low24 = arg32.low24; ; return rval; ;} ; ; ;uns8 operator=( sharedM float24 arg1f24) @ ;int8 operator=( sharedM float24 arg1f24) @ ;uns16 operator=( sharedM float24 arg1f24) @ ;int16 operator=( sharedM float24 arg1f24) @ ;int24 operator= _float24ToInt24( sharedM float24 arg1f24) ;{ _float24ToInt24 ; sharedM float24 arg2f24; // unused, but required ; char sign = arg1f24.mid8; MOVFF arg1f24+1,sign_6 ; char expo = arg1f24.high8 - (FpBIAS-1); MOVLW 126 SUBWF arg1f24+2,W,0 MOVWF expo_4,0 ; if (!Carry) ; goto RES0; BNC m056 ; arg1f24.15 = 1; BSF arg1f24+1,7,0 ; ; arg1f24.high8 = 0; CLRF arg1f24+2,0 ; #ifndef DISABLE_ROUNDING ; char xtra = 0; CLRF xtra_4,0 ; #endif ; ; // (a): expo = 0..8 : shift 1 byte to the right ; // (b): expo = 9..16: shift 0 byte ; // (c): expo = 17..24: shift 1 byte to the left ; #if __CoreSet__ / 100 == 12 ; expo -= 17; ; expo = 0xFF - expo; // COMF (Carry unchanged) ; if (Carry) { // (c) ; #else ; expo = 16 - expo; SUBLW 16 MOVWF expo_4,0 ; if (!Carry) { // (c) BC m051 ; #endif ; expo += 8; MOVLW 8 ADDWF expo_4,1,0 ; if (!Carry) ; goto OVERFLOW; BNC m055 ; arg1f24.high8 = arg1f24.mid8; MOVFF arg1f24+1,arg1f24+2 ; arg1f24.mid8 = arg1f24.low8; MOVFF arg1f24,arg1f24+1 ; arg1f24.low8 = 0; CLRF arg1f24,0 ; } ; else { // (a) (b) BRA m052 ; // expo = 0 .. 16 ; W = expo - 8; m051 MOVLW 8 SUBWF expo_4,W,0 ; if (Carry) { // (a) BNC m052 ; expo = W; MOVWF expo_4,0 ; #ifndef DISABLE_ROUNDING ; xtra = arg1f24.low8; MOVFF arg1f24,xtra_4 ; #endif ; arg1f24.low8 = arg1f24.mid8; MOVFF arg1f24+1,arg1f24 ; arg1f24.mid8 = 0; CLRF arg1f24+1,0 ; } ; } ; if (expo) { m052 MOVF expo_4,1,0 BZ m054 ; do { ; Carry = 0; m053 BCF 0xFD8,Carry,0 ; arg1f24.high8 = rr( arg1f24.high8); RRCF arg1f24+2,1,0 ; arg1f24.low16 = rr( arg1f24.low16); RRCF arg1f24+1,1,0 RRCF arg1f24,1,0 ; #ifndef DISABLE_ROUNDING ; xtra = rr( xtra); RRCF xtra_4,1,0 ; #endif ; } while (--expo); DECFSZ expo_4,1,0 BRA m053 ; } ; if (arg1f24.23) { m054 BTFSS arg1f24+2,7,0 BRA m058 ; OVERFLOW: ; FpOverflow = 1; m055 BSF 0x0A,FpOverflow,0 ; W = 0xFF; MOVLW 255 ; goto ASSIGNW; BRA m057 ; RES0: ; W = 0; m056 MOVLW 0 ; ASSIGNW: ; arg1f24.low8 = W; m057 MOVWF arg1f24,0 ; arg1f24.mid8 = W; MOVWF arg1f24+1,0 ; arg1f24.high8 = W; MOVWF arg1f24+2,0 ; arg1f24.23 = 0; BCF arg1f24+2,7,0 ; } ; else { BRA m060 ; #ifndef DISABLE_ROUNDING ; if (FpRounding && (xtra & 0x80)) { m058 BTFSS 0x0A,FpRounding,0 BRA m059 BTFSS xtra_4,7,0 BRA m059 ; arg1f24.low8 += 1; INFSNZ arg1f24,1,0 ; if (!arg1f24.low8) ; arg1f24.mid8 += 1; INCF arg1f24+1,1,0 ; } ; #endif ; if (sign & 0x80) m059 BTFSS sign_6,7,0 BRA m060 ; arg1f24.low24 = -arg1f24.low24; BSF 0xFD8,Carry,0 MOVLW 0 SUBFWB arg1f24,1,0 SUBFWB arg1f24+1,1,0 SUBFWB arg1f24+2,1,0 ; } ; int24 rval @ arg1f24; ; rval = arg1f24.low24; ; return rval; m060 MOVF rval_3,W,0 RETURN ;} ; ; ;bit operator< _f24_LT_f24( sharedM float24 arg1f24, sharedM float24 arg2f24) ;{ _f24_LT_f24 ; Carry = 0; ; if (!(arg1f24.high8 | arg2f24.high8)) ; return Carry; ; if (!arg1f24.15) { ; if (arg2f24.15) ; return Carry; ; W = arg1f24.low8 - arg2f24.low8; ; genSubW( arg1f24.mid8, arg2f24.mid8); ; genSubW( arg1f24.high8, arg2f24.high8); ; goto RETURN; ; } ; if (!arg2f24.15) ; goto RETURN; ; W = arg2f24.low8 - arg1f24.low8; ; genSubW( arg2f24.mid8, arg1f24.mid8); ; genSubW( arg2f24.high8, arg1f24.high8); ; RETURN: ; if (Carry) ; return 0; ; return 1; ;} ; ; ;bit operator>= _f24_GE_f24( sharedM float24 arg1f24, sharedM float24 arg2f24) ;{ _f24_GE_f24 ; Carry = 1; ; if (!(arg1f24.high8 | arg2f24.high8)) ; return Carry; ; if (!arg1f24.15) { ; if (arg2f24.15) ; return Carry; ; W = arg1f24.low8 - arg2f24.low8; ; genSubW( arg1f24.mid8, arg2f24.mid8); ; genSubW( arg1f24.high8, arg2f24.high8); ; return Carry; ; } ; Carry = 0; ; if (!arg2f24.15) ; return Carry; ; W = arg2f24.low8 - arg1f24.low8; ; genSubW( arg2f24.mid8, arg1f24.mid8); ; genSubW( arg2f24.high8, arg1f24.high8); ; return Carry; ;} ; ; ; ;bit operator> _f24_GT_f24( sharedM float24 arg1f24, sharedM float24 arg2f24) ;{ _f24_GT_f24 ; Carry = 0; ; if (!(arg1f24.high8 | arg2f24.high8)) ; return Carry; ; if (!arg1f24.15) { ; if (arg2f24.15) ; goto RETURN; ; W = arg2f24.low8 - arg1f24.low8; ; genSubW( arg2f24.mid8, arg1f24.mid8); ; genSubW( arg2f24.high8, arg1f24.high8); ; goto RETURN; ; } ; if (!arg2f24.15) ; return Carry; ; W = arg1f24.low8 - arg2f24.low8; ; genSubW( arg1f24.mid8, arg2f24.mid8); ; genSubW( arg1f24.high8, arg2f24.high8); ; RETURN: ; if (Carry) ; return 0; ; return 1; ;} ; ; ; ;bit operator<= _f24_LE_f24( sharedM float24 arg1f24, sharedM float24 arg2f24) ;{ _f24_LE_f24 ; Carry = 1; ; if (!(arg1f24.high8 | arg2f24.high8)) ; return Carry; ; if (!arg1f24.15) { ; Carry = 0; ; if (arg2f24.15) ; return Carry; ; W = arg2f24.low8 - arg1f24.low8; ; genSubW( arg2f24.mid8, arg1f24.mid8); ; genSubW( arg2f24.high8, arg1f24.high8); ; return Carry; ; } ; if (!arg2f24.15) ; return Carry; ; W = arg1f24.low8 - arg2f24.low8; ; genSubW( arg1f24.mid8, arg2f24.mid8); ; genSubW( arg1f24.high8, arg2f24.high8); ; return Carry; ; FILE 18\demo-fpm.c ; ;#pragma chip PIC18C252 ;#include "math24f.h" ;uns16 ad_data; ;float tx, av, mg, a, vx, prev, kp; ; ;void main(void) ;{ main ; InitFpFlags(); // enable rounding as default MOVLW 64 MOVWF FpFlags,0 ; vx = 3.127; MOVLW 33 MOVWF vx,0 MOVLW 72 MOVWF vx+1,0 MOVLW 128 MOVWF vx+2,0 ; tx += ad_data; // automatic type cast MOVFF ad_data,arg1f24 MOVFF ad_data+1,arg1f24+1 CLRF arg1f24+2,0 RCALL _int24ToFloat24 MOVFF tx,arg2f24 MOVFF tx+1,arg2f24+1 MOVFF tx+2,arg2f24+2 RCALL _fadd24 MOVFF arg1f24,tx MOVFF arg1f24+1,tx+1 MOVFF arg1f24+2,tx+2 ; ad_data = kp; // assign integer part MOVFF kp,arg1f24 MOVFF kp+1,arg1f24+1 MOVFF kp+2,arg1f24+2 RCALL _float24ToInt24 MOVFF rval_3,ad_data MOVFF rval_3+1,ad_data+1 ; if (tx < 0) MOVF tx+2,W,0 BZ m061 BTFSC tx+1,7,0 ; tx = -tx; // make positive BTG tx+1,7,0 ; av = tx/20.0; m061 MOVFF tx,arg1f24 MOVFF tx+1,arg1f24+1 MOVFF tx+2,arg1f24+2 CLRF arg2f24,0 MOVLW 32 MOVWF arg2f24+1,0 MOVLW 131 MOVWF arg2f24+2,0 RCALL _fdiv24 MOVFF arg1f24,av MOVFF arg1f24+1,av+1 MOVFF arg1f24+2,av+2 ; mg = av * 1.25; MOVFF av,arg1f24 MOVFF av+1,arg1f24+1 MOVFF av+2,arg1f24+2 CLRF arg2f24,0 MOVLW 32 MOVWF arg2f24+1,0 MOVLW 127 MOVWF arg2f24+2,0 RCALL _fmul24 MOVFF arg1f24,mg MOVFF arg1f24+1,mg+1 MOVFF arg1f24+2,mg+2 ; a = mg * 0.98; MOVFF mg,arg1f24 MOVFF mg+1,arg1f24+1 MOVFF mg+2,arg1f24+2 MOVLW 225 MOVWF arg2f24,0 MOVLW 122 MOVWF arg2f24+1,0 MOVLW 126 MOVWF arg2f24+2,0 RCALL _fmul24 MOVFF arg1f24,a MOVFF arg1f24+1,a+1 MOVFF arg1f24+2,a+2 ; prev = vx; MOVFF vx,prev MOVFF vx+1,prev+1 MOVFF vx+2,prev+2 ; vx = a/5.0 + prev; MOVFF a,arg1f24 MOVFF a+1,arg1f24+1 MOVFF a+2,arg1f24+2 CLRF arg2f24,0 MOVLW 32 MOVWF arg2f24+1,0 MOVLW 129 MOVWF arg2f24+2,0 RCALL _fdiv24 MOVFF prev,arg2f24 MOVFF prev+1,arg2f24+1 MOVFF prev+2,arg2f24+2 RCALL _fadd24 MOVFF arg1f24,vx MOVFF arg1f24+1,vx+1 MOVFF arg1f24+2,vx+2 ; ; kp = vx * 0.036; MOVFF vx,arg1f24 MOVFF vx+1,arg1f24+1 MOVFF vx+2,arg1f24+2 MOVLW 117 MOVWF arg2f24,0 MOVLW 19 MOVWF arg2f24+1,0 MOVLW 122 MOVWF arg2f24+2,0 RCALL _fmul24 MOVFF arg1f24,kp MOVFF arg1f24+1,kp+1 MOVFF arg1f24+2,kp+2 ; kp = vx / (1.0/0.036); MOVFF vx,arg1f24 MOVFF vx+1,arg1f24+1 MOVFF vx+2,arg1f24+2 MOVLW 57 MOVWF arg2f24,0 MOVLW 94 MOVWF arg2f24+1,0 MOVLW 131 MOVWF arg2f24+2,0 RCALL _fdiv24 MOVFF arg1f24,kp MOVFF arg1f24+1,kp+1 MOVFF arg1f24+2,kp+2 ;} SLEEP RESET END ; *** KEY INFO *** ; 0x000004 82 word(s) 0 % : _fmul24 ; 0x0000A8 87 word(s) 0 % : _fdiv24 ; 0x000156 132 word(s) 0 % : _fadd24 ; 0x00025E 56 word(s) 0 % : _int24ToFloat24 ; 0x0002CE 65 word(s) 0 % : _float24ToInt24 ; 0x000350 168 word(s) 1 % : main ; RAM usage: 34 bytes (10 local), 1502 bytes free ; Maximum call level: 1 ; Total of 592 code words (3 %)