test/kalmanTools/makePTPhiLUTs.py

0001 from __future__ import print_function
0002 from math import pi,floor
0003
0004
0005 print(int(((-330+1024)*pi/(6.0*2048.0))/(0.625*pi/180.0)))
0006
0007
0008
0009 #phi=[]
0010 #for i in range(0,2048):
0011 #    p = int((i*pi/(6.0*2048.0)+15.0*pi/180.0)/(0.625*pi/180.0))
0012 #    p = int((i*2*pi/(6.0*2048.0))/(0.625*pi/180.0))
0013 #    phi.append(str(p))
0014
0015 #print('const ap_int<8> phiLUT[2047] = {'+','.join(phi)+'};')
0016 #import pdb;pdb.set_trace()
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026 def bits(number, size_in_bits):
0027     """
0028     The bin() function is *REALLY* unhelpful when working with negative numbers.
0029     It outputs the binary representation of the positive version of that number
0030     with a '-' at the beginning. Woop-di-do. Here's how to derive the two's-
0031     complement binary of a negative number:
0032
0033         complement(bin(+n - 1))
0034
0035     `complement` is a function that flips each bit. `+n` is the negative number
0036     made positive.
0037
0038     """
0039     if number < 0:
0040         return compliment(bin(abs(number) - 1)[2:]).rjust(size_in_bits, '1')
0041     else:
0042         return bin(number)[2:].rjust(size_in_bits, '0')
0043
0044 def compliment(value):
0045     return ''.join(COMPLEMENT[x] for x in value)
0046
0047 COMPLEMENT = {'1': '0', '0': '1'}
0048
0049
0050
0051
0052
0053
0054
0055 phiLUT=[]
0056 kPHI = 57.2958/0.625/1024.;
0057
0058 for i in range(0,1024):
0059     phiLUT.append(0)
0060 for phi in range(-512,512):
0061     address = int(bits(phi,10),2)
0062     phiF=float(phi)
0063     phiNew = 24+int(floor(kPHI*phiF));
0064     if phiNew >  69:
0065         phiNew =  69;
0066     if phiNew < -8:
0067         phiNew = -8;
0068     phiLUT[address]=(str(phiNew))
0069 print('const ap_int<8> phiLUT[1024] = {'+','.join(phiLUT)+'};')
0070
0071
0072
0073 ptLUT=[]
0074 lsb = 1.25/(1<<13)
0075 for i in range(0,4096):
0076     ptLUT.append(6)
0077 for K in range(-2048,2048):
0078     address = int(bits(K,12),2)
0079     if K>=0:
0080         charge=1
0081     else:
0082         charge=-1
0083
0084
0085
0086     FK=lsb*abs(K)
0087
0088     if abs(K)>2047:
0089         FK=lsb*2047
0090
0091     if abs(K)<26:
0092         FK=lsb*26
0093
0094     FK = 0.898*FK/(1.0-0.6*FK);
0095     FK=FK-26.382*FK*FK*FK*FK*FK;
0096     FK=FK-charge*1.408e-3;
0097     FK=FK/1.17;
0098     if (FK!=0.0):
0099         pt=int(2.0/FK)
0100     else:
0101         pt=511
0102
0103     if pt>511:
0104         pt=511
0105
0106     if pt<6:
0107         pt=6;
0108     ptLUT[address]=str(pt)
0109
0110
0111
0112
0113 print('const ap_uint<9> ptLUT[4096] = {'+','.join(ptLUT)+'};')