CON _xtlfreq = 20_000_000 _clkfreq = 10_000_000 DOWNLOAD_BAUD = 230_400 DEBUG_BAUD = DOWNLOAD_BAUD DIAGTXPIN = 62 DIAGRXPIN = 63 tickus = (_clkfreq + 500_000) / 1_000_000 tickms = (_clkfreq + 500) / 1_000 PUB baudinit( baud ) | x x := muldiv65( 64, clkfreq, baud ) << 10 | 7 pinstart( DIAGTXPIN, P_ASYNC_TX | P_OE, x, 0 ) pinstart( DIAGRXPIN, P_ASYNC_RX, x, 0 ) PUB putch( char ) org .loop rqpin inb, #DIAGTXPIN wc 'transmiting? (C high == yes) *Needed to initiate tx testp #DIAGTXPIN wz 'buffer free? (IN high == yes) if_c_and_nz jmp #.loop 'wait while Smartpin is both full (nz) and transmitting (c) wypin char, #DIAGTXPIN 'write new byte to Y buffer end PUB str( addr ) | c repeat while (c := byte[addr++]) <> 0 send( c ) PUB hex( value, place ) | digit place := (place - 1) << 2 repeat digit := value >> place & 15 place -= 4 if digit < 10 send( "0" + digit ) else send( "a" + digit - 10 ) until place < 0 PUB deci( value ) if value < 0 send("-") value := -value udeci( value ) PUB udeci( value ) | flag, place, digit flag~ place := 1_000_000_000 repeat digit := value +/ place +// 10 flag ||= digit || place == 1 if flag send("0" + digit) while place +/= 10 PUB decn( value, places ) if value < 0 send("-") value := -value udecn( value, places ) PUB udecn( value, places ) | flag, place, digit flag~ place := 1_000_000_000 repeat digit := value +/ place +// 10 flag ||= digit || place == 1 if flag send("0" + digit) elseif place <= places send(" ") while place +/= 10 { PUB dpt( value, places ) | flag, place, digit, value2 'places is decimal point position if value < 0 send("-") value := -value flag~ places #>= 1 place := 1_000_000_000 value2 := value / places value //= places repeat if flag ||= (digit := value2 / place // 10) || place == 1 send("0" + digit) while place /= 10 send(".") if places /= 10 repeat digit := value / places // 10 send("0" + digit) while places /= 10 } { PUB div33ieee( dividend, divisor ) : r | s org mov r, divisor shr r, #1 wc ' odd(1)/even(0), even divisors all produce exact 0.5 results add dividend, r ' round to nearest: + 50% divisor rep @.rend, #1 ' IRQ shield qdiv dividend, divisor if_nc sub dividend, #1 ' potential alternate, from even divisor if_nc qdiv dividend, divisor if_nc getqx s if_nc testb s, #0 wc ' odd(1)/even(0) getqx r .rend if_nc mov r, s ' even divisor and result, keep even result end PUB muldiv65ieee( mult1, mult2, divisor ) : r | s org rep @.rend1, #1 ' IRQ shield qmul mult1, mult2 ' 4 mov r, divisor shr r, #1 ' r = 50% divisor testb divisor, #0 wz ' odd(1)/even(0), even divisors all produce exact 0.5 results getqx mult1 ' 60 getqy mult2 .rend1 add mult1, r wc ' round to nearest: + 50% divisor addx mult2, #0 rep @.rend2, #1 ' IRQ shield setq mult2 qdiv mult1, divisor ' 76 if_nz sub mult1, #1 wc ' potential alternate, from even divisor if_nz subx mult2, #0 if_nz setq mult2 if_nz qdiv mult1, divisor ' 84 if_nz getqx s ' 132 if_nz testb s, #0 wz ' odd(1)/even(0) getqx r ' 140 .rend2 if_nz mov r, s ' 142, even divisor and result, keep even result end } PUB div33( dividend, divisor ) : result ' Round to nearest 32-bit divide org mov result, divisor shr result, #1 add dividend, result qdiv dividend, divisor getqx result end PUB muldiv65( mult1, mult2, divisor ) : result ' 32bit result = 32bit x 32bit / 32bit ' Rounding to nearest variant of muldiv64() org qmul mult1, mult2 mov result, divisor shr result, #1 getqx mult1 ' lower 32 bits of 64-bit intermediate getqy mult2 ' upper 32 bits of 64-bit intermediate add mult1, result wc ' round-up addx mult2, #0 ' round-up setq mult2 qdiv mult1, divisor getqx result end pub law_of_cosines( a, b, c ) : angle | hyp, opp ' apply law of cosines to input length values where a, b, c are the lengths of the triangle sides ' return angle, C, is angle between a and b sides in 10ths of degrees ' Formula: c2 = a2 + b2 - 2ab cos(C) ' => 2ab cos(C) = a2 + b2 - c2 ' => cos(C) = (a2 + b2 - c2) / 2ab ... or Cos(x) = adjacent / hypotenuse org mov hyp, a mul hyp, b shl hyp, #1 ' a * b * 2 => hypotenuse mul a, a ' square the a parameter mul b, b ' square the b parameter mul c, c ' square the c parameter add a, b ' add a2 + b2 sub a, c ' subtract c2 => adjacent 'However, QVECTOR requires adjacent and opposite rather than hypotenuse ' therefore extract opposite from adjacent and hypotenuse using Pythagoras Theorem: adjacent2 + opposite2 = hypotenuse2 ' => b2 = c2 - a2 'using QMUL encod b, a encod c, hyp fge b, c subr b, #30 wcz ' if highest is below 31 bits then scale to 31 bits if_a shl a, b if_a shl hyp, b qmul hyp, hyp ' square hypotenuse qmul a, a ' square adjacent getqx opp ' hypotenuse squared getqy angle getqx c ' adjacent squared getqy b sub opp, c wcz ' opp2 = hyp2 - adj2 subx angle, b ' upper bits qsqrt opp, angle ' squareroot opposite2 getqx opp ' opposite qvector a, opp ' adjacent, opposite getqy angle ' 32-bit angle qmul angle, ##360_000001 ' degrees with 6 decimal places getqy angle end pub pllset( targetfreq ) : xinfreq | mode, mult, divd, divp, post, Fpfd, Fvco, error, besterror, vcolimit mode := clkmode_ if clkmode_ >> 24 & 1 ' compiled with PLL on divd := clkmode_ >> 18 & $3f + 1 mult := clkmode_ >> 8 & $3ff + 1 divp := (clkmode_ >> 4 + 1) & $f divp := divp ? divp * 2 : 1 xinfreq := muldiv65( divp * divd, clkfreq_, mult ) elseif clkmode_ >> 2 & 3 ' compiled with PLL off xinfreq := clkfreq_ ' clock pass-through else ' unknown build mode xinfreq := 20_000_000 ' default to 20 MHz crystal mode := %10_00 ' default to 15 pF loading mode := %11_11 & mode | %11 ' keep %CC, force %SS, ditch the rest besterror := div33( targetfreq, 100 ) ' _errfreq at 1.0% of targetfreq vcolimit := targetfreq + besterror vcolimit := vcolimit < 201_000_000 ? 201_000_000 : vcolimit repeat post from 0 to 15 divp := post ? post * 2 : 1 repeat divd from 64 to 1 Fpfd := div33( xinfreq, divd ) mult := muldiv65( divp * divd, targetfreq, xinfreq ) Fvco := muldiv65( xinfreq, mult, divd ) if Fpfd >= 250_000 and mult <= 1024 and Fvco > 99_000_000 and Fvco <= vcolimit error := div33( Fvco, divp ) - targetfreq if abs( error ) <= abs( besterror ) ' the last iteration at equality gets priority besterror := error mode := (mode&%11_11) | 1<<24 | (divd-1)<<18 | (mult-1)<<8 | ((post-1)&15)<<4 if mode.[24] ' PLL-ON bit set when calculation is valid clkset( mode, targetfreq + besterror ) ' make the frequency change ' baudval() ' recalibrate debug comms as well else xinfreq := -1 ' failed, no change CON { license } {{ Terms of Use: MIT License Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. }}