'MLX90641 IR camera support
 
 
 '-------------------- generic program defines ------------------------
 
 const SA=&h33    'I2C 7 bit slave address is &h5A
 I2Cport=1                 'I2C2=2 anything other is I2C
 scalefactor=4             '2=16x12 to 31x23
 num_colors=16              '6 or 8  'define video interface
 dim i,x,y as integer      'in stead of replacing all i with i%
 
 
 '-------------------------system initialisation ----------------------
 
 vga%=instr(mm.device$,"VGA")          '0 or not 0
 if vga% then
   mode 2
 else
   font 7
 endif
 framebuffer create     'use framebuffer when LCD faster screen
 framebuffer write F    'use framebuffer when LCD faster screen
 
 
 'I2C bus open at 400kHz
 if I2Cport=2 then
   'assume system I2C on GP14/GP15
   'setpin gp14,gp15,I2C2
   'I2C2 open 400,100
 else
   setpin gp0,gp1,I2C
   I2C open 400,100
 end if
 
 
 'arrays with values per pixel
 dim Po1(191)  'offset subbank 1
 dim Po0(191)  'offset subbank 0
 dim Pa(191)   'alpha
 dim Pkt(191)  'temperature impact
 dim Pkv(191)  'voltage impact
 dim Vir(191)  'voltage from sensor element and after conversion temperature
 dim Toffs(191):math set 2,Toffs()'Temperature offsets for manual calibration
 
 'debugging how much memory left
 'dim vzz(250) '2kb
 
 'define the colors
 if num_colors=6 then 'colors
   dim col%(5) = (rgb(blue),rgb(cerulean),rgb(red),rgb(orange),rgb(yellow),rgb(white)) 'tile colors
 else  if num_colors=8  then '8 colors
   dim col%(7) = (rgb(magenta),rgb(blue),rgb(cyan),rgb(green),rgb(yellow),rgb(orange),rgb(red),rgb(white))
 else '16 colors
   dim col%(15)=(0,&h4000,&h8000,&hff,&h40ff,&h80ff,&hff00ff,&hff40ff,&hff80ff,&hff0000,&hff4000,&hff8000,&hFFFF00,&hFF00,&hFFFF,&hFFFFFF)
 end if
 
 
 'scaling buffers and math matrices
 if scalefactor=2 then
   Dim scrn(31,22)    '16*2-1,12*2-1 = 31x23 screen pixels + 1 dummy
 else 'scalefactor = 4 (uses matrix math)
   dim Toa(15,11)                            'this is Vir() in 2D form
   dim b(61,15),c(15,44),d(11,44)            'arrays for matrix math
   fill_bd_4                                 'conversion matrices
   dim scrn(61,44)    '16*4-3 x 12*4-3 = 61x45 screen pixels + 1 dummy column
 end if
 
 memory 'debug
 
 '--------------------------- read camera constants -------------------
 
 get_res_corr
 dummy=get_vdd()
 get_cal
 get_Kta_Kv
 get_Po0_Po1
 get_Pa
 get_Vir
 get_conf
 get_stat
 
 
 '---------------------------- screen setup -----------------------------
 
 'place the legend on screen
 box 310,47,10,179,1,0,0               'bar graph frame
 v_size = 179 \ num_colors
 
 for i=0 to num_colors-1
   box 311,48+i*v_size,8,v_size,1,col%(num_colors-1-i),col%(num_colors-1-i)
 next
 
 text 0,232,"PicoMite Thermal Camera, C = cal, S = save, Q = quit",,8
 
 
 '-------------------------- MAIN LOOP -------------------------------
 'this is based on MLX90641 datasheet chapter 10.5 without STEP mode
 'the MLX90641 runs autonomous at 500ms (2Hz) default speed
 
 do
   
   'wait for new data by checking data available flag
   do
     pause 10
     status% = get_reg(&h8000)
   loop until (status% and 8) = 8
   
   
   'get actual subpage where data is available
   page% = (status% and 1)
   
   'get data (takes roughly 190ms)
   get_vir(page%)
   
   'clear data ready bit (just write the page bit back)
   set_reg(&h8000,page%)
   
   'convert IR readings to temperature according datasheet
   temp_conv
   
   
   'check if we need to re-calibrate
   if a$="c" then cal_array
   
   'calibrate the measured values to the reference
   math c_sub Vir(),Toffs(),Vir()

   'calculate extremes
   tmax=math(max Vir(),pix%):tmin=math(min Vir())
   Tlow=min(tmin,Ta-2):Thigh=max(tmax,Tlow+10)


   'show the colorfull picture on screen
   if scalefactor=2 then
     scale_up
   else
     scale_4
   end if
   
   
   'show the legend
   
   box 260,0,60,16,1,rgb(white),rgb(black)         'peak value in view
   text 270,4,"X "+str$(tmax,3,1),,,,rgb(white),rgb(black)
   
   box 260,17,60,16,1,rgb(white),rgb(black)         'centre screen value
   text 282,21,str$(Vir(88),3,1),,,,rgb(white),rgb(black)
   circle 272,24,5,,,rgb(pink)
   
   box 280,34,40,13,1,rgb(black),rgb(white)        'high value
   text 284,37,str$(Thigh,3,1),,,,rgb(black),rgb(white)
   
   box 280,226,40,14,1,rgb(black),rgb(blue)        'low value
   text 284,229,str$(Tlow,3,1),,,,rgb(white),rgb(blue)
   
   
   'the marker's
   text 20*(pix% mod 16)-2,20*(pix%\16)-3,"X",,1   'cross
   circle 162,102,10,,,rgb(pink)                   'ring
   
   'debug
   text 0,0,str$(timer,3,0):timer=0
   
   'fast copy of framebuffer to screen
   framebuffer copy f,n

   
   'do we want to save the current image ?
   if a$="s" then save image "screenshot.bmp"
   a$=inkey$
   
 loop while a$<>"q"
 
end
 
sub temp_conv
 
 local f_t(191)  'scratchpad temp
 local f_v(191)  'scratchpad voltage
 
 'process data according 11.2.2 from datasheet
 Vdd=get_vdd()                     'get Vdd 11.2.2.2
 Ta=get_ta()                       'get Ta 11.2.2.3
 
 math scale vir(),Kgain,vir()      'scale gain 11.2.2.5.1
 
 'IRdatcomp 11.2.2.5.3
 math scale pkt(),Ta-25,f_t()      'calc 1+Kta*(Ta-T25)
 math add f_t(),1,f_t()
 math scale pkv(),vdd-3.3,f_v()    'calc 1+Kv*(Vdd-3.3)
 math add f_v(),1,f_v()
 math c_mul f_v(),f_t(),f_v()      'multiply and result in f_v()
 if page% then                     'compensate for page offset
   math c_mul f_v(),Po1(),f_v()
 else
   math c_mul f_v(),Po0(),f_v()
 end if
 math c_sub Vir(),f_v(),Vir()      'and add to pixel voltage
 
 'emissivity comp 11.2.2.5.4 and IR gradient comp 11.2.2.7
 math add Vir(),-tgc*cp_pix_os,Vir()
 math scale Vir(),1/em,Vir()             'this is Vir_compensated()
 
 'Sensitivity normalize 11.2.2.8 and put values in f_t() for temp conversion
 math add Pa(),-tgc*alpha_cp,f_t()
 math scale f_t(),1+ksta*(Ta-25),f_t()   'this is alpha_comp()
 
 
 'Temp conversion (0-80C) 11.2.2.9
 Tark4=calc_tark()
 'since there are no math commands for square root and raise to the power
 'this is still in a loop, needs 55ms @ 252MHz (all above math is only 10ms)
 for i=0 to 191
   Sx=KsTo_3*( ( Vir(i)*(f_t(i)^3) + (f_t(i)^4)*Tark4 )^(1/4) )
   Vir(i)=((tark4 + Vir(i)/(f_t(i)*(1-KsTo_3*273.15)+Sx))^(1/4))-273.15
 next
 
end sub
 
 '--------------------------- display routines -------------------------------
 'these routines perform the visual representation of the data in Tob()
 
 'show 192 temperatures on screen in a grid of 31x23 interleaved
 
sub scale_up '2x
 
 local bc%,xm%
 
 'timer=0
 
 'seed the measurement data Tob()on the screen window scrn()
 'scaling math takes roughly 65ms this way
 
 for y=0 to 11
   for x=0 to 15
     scrn(2*x,2*y)=Vir(x+16*y)
   next
 next
 
 'interleave the horizontal even lines
 for y=0 to 22 step 2
   for x=1 to 29 step 2
     scrn(x,y)=(scrn(x-1,y)+scrn(x+1,y))/2
   next
 next
 
 'interleave the horizontal odd lines
 for y=1 to 21 step 2
   for x=0 to 30
     scrn(x,y)=(scrn(x,y-1)+scrn(x,y+1))/2
   next
 next
 
 'convert to colors (only picomite 5.08.00 and newer)
 math window scrn(),0,num_colors-1,scrn()   '6 or 8 colors
 
 'block size for 31x23 pixels on 320x240 screen
 size%=10
 
 '? timer,
 
 'show screen on mode 2 on picomite (takes roughly 130ms)
 'box 0,0,310,220,1,col%(com_col%),col%(com_col%)
 for x=0 to 30
   xm%=x*size%
   for y=0 to 22
     bc%=col%(scrn(x,y))
     box x*size%,y*size%,size%,size%,1,bc%,bc%
   next
 next
 
 'put some default values in the array
 math set Thigh,scrn():scrn(31,0)=Tlow
 
 '? timer
 
end sub
 
 'show 192 temperatures on screen in a grid of 61x45 interleaved
sub scale_4
 
 local bc%,xm%
 
 'timer=0
 
 'copy linear array into 16x12 array, and perform math to scale up (38ms)
 for y=0 to 11
   for x=0 to 15
     Toa(x,y)=Vir(x+16*y)
   next
 next
 math m_mult d(),Toa(),c()           'c() is correct vertical resolution
 math m_mult c(),b(),scrn()
 
 'set the pixels in the dummy column to smooth the screen colors when no
 'delta T is visible
 for i=1 to 44:scrn(61,i)=Thigh:next :scrn(61,0)=Tlow
 
 
 'convert to colors (only picomite 5.08.00 and newer)
 math window scrn(),0,num_colors-1,scrn()   '8 colors
 
 'block size for 61x45 pixels on 320x240 screen
 size%=5
 
 '? timer,
 
 'show screen on mode 2 on picomite (takes roughly 350ms)
 for x=0 to 60
   xm%=x*size%
   for y=0 to 44
     bc%=col%(scrn(x,y))
     box xm%,y*size%,size%,size%,1,bc%,bc%
   next
 next
 
 '? timer
 
end sub
 
 
sub cal_array
 math add Vir(),-Ta,Toffs()
end sub
 
 
 
 ' ------------------- get pixel cal data from the chip ----------------------
 'these functions read data from the MLX90641 and do some formatting
 
 
 'get status register
sub get_stat
 
 'get value
 Reg8000%=get_reg(&h8000)
 
end sub
 
 'get config register
sub get_conf
 
 'get value
 Reg800D%=get_reg(&h800D)
 
end sub
 
 
 'get voltages for the pixel array from subpage n (0 or 1)
sub get_vir n
 local i,j,yoff,xoff as integer
 
 xoff=n*&h20
 
 for j=0 to 5      'all 6 banks
   yoff=j*&h40
   for i=0 to 31   'of 32 pixels
     'get value
     Vir(i+j*32)=get_reg_16_2(&h400 + i + yoff + xoff)
   next
 next
 
end sub
 
 
 'get alpha(i) and adjust for row (0-5)
sub get_Pa
 local i as integer
 
 'adjust per row (32 pix) according 11.2.2.8
 for i=0 to 31
   Pa(i)=a_ref_row_1*get_reg_11(&h2500+i)/2047
 next
 for i=32 to 63
   Pa(i)=a_ref_row_2*get_reg_11(&h2500+i)/2047
 next
 for i=64 to 95
   Pa(i)=a_ref_row_3*get_reg_11(&h2500+i)/2047
 next
 for i=96 to 127
   Pa(i)=a_ref_row_4*get_reg_11(&h2500+i)/2047
 next
 for i=128 to 159
   Pa(i)=a_ref_row_5*get_reg_11(&h2500+i)/2047
 next
 for i=160 to 191
   Pa(i)=a_ref_row_6*get_reg_11(&h2500+i)/2047
 next
 
end sub
 
 'get Kta(i) and Kv(i) and scale them to be used for math
sub get_Kta_Kv
 local x,i as integer
 
 for i=0 to 191 'all 192 pixels
   x=&h25c0+i
   
   'get value
   Pkt(i)=(get_reg(x) and &h7e0)/32
   if Pkt(i)>31 then inc Pkt(i),-64
   
   'scale
   Pkt(i)=(Pkt(i)*(2^kta_scale_2) + kta_avg) / (2^kta_scale_1)
   
   'get value
   Pkv(i)=get_reg(x) and &h1f
   if Pkv(i)>15 then inc Pkv(i),-32
   
   'scale
   Pkv(i)=(Pkv(i)*(2^kv_scale_2) + kv_avg) / (2^kv_scale_1)    
 next
 
end sub
 
 'get Offset(i) for 0 and 1 compensated as in 11.2.2.5.2
sub get_Po0_Po1
 local x,i as integer
 
 for i=0 to 191 'all 192 pixels
   x=&h2440+i
   
   'get value
   Po0(i) = get_reg_11_2(x)
   
   'scale
   Po0(i) = (Po0(i) * 2^scale_os_r1) + pix_os_av  
 next
 
 for i=0 to 191 'all 192 pixels
   x=&h2680+i
   
   'get value
   Po1(i) = get_reg_11_2(x)
   
   'scale
   Po1(i) = (Po1(i) * 2^scale_os_r2) + pix_os_av    
 next
 
end sub
 
 'determine Tambient
function get_Ta()
 local dv,vbe,vptat,vptat_art as float
 
 dv = get_vdd()-3.3
 vbe = get_reg_16_2(&h580)
 vptat = get_reg_16_2(&h5a0)
 vptat_art = (vptat / (vptat*alpha_ptat + vbe)) * 2^18
 
 'formula from datasheet 11.1.2
 get_Ta = (((vptat_art/(1+kv_ptat*dv))-ptat)/kt_ptat)+25
 
end function
 
function calc_tark()
 Tr=Ta-5 'from datasheet in absence of better
 local tvk4=(Tr+273.15)^4
 calc_tark=tvk4-(tvk4-(Ta+273.15)^4)/em
end function
 
 
 
 ' ------------------- get specific data from the chip ------------------------
 'these functions read data from the MLX90641 and do some formatting
 
 
 'resolution correction from the ADC
sub get_res_corr
 RC = (get_reg(&h2433) and &h600)>>9
 RC = RC/((get_reg(&h800D) and &hC00)>>10)
 if debug then print "RC               = ";RC
end sub
 
 
 'actual Vdd measured by the chips ADC
function get_vdd() as float
 local v=get_reg_16_2(&h5aa), k=get_reg_11_2(&h2427), va=get_reg_11_2(&h2426)
 va=va*32 : k=k*32
 get_vdd = 3.3 + (RC*v - va)/k
 if debug then print "Vdd              = ";get_vdd
end function
 
 
 'this sub extracts calibration values from the chips EEPROM and formats these
 'for use in the math
 'formula's are derived from MLX90641 data sheets
sub get_cal
 local x
 local scale_row1,row1_max
 local scale_row2,row2_max
 local scale_row3,row3_max
 local scale_row4,row4_max
 local scale_row5,row5_max
 local scale_row6,row6_max
 local alpha_cp_sc, kta_cp_sc, kv_cp_sc
 
 
 x=get_reg_11(&h2410)
 scale_os_r1 = x>>5 : scale_os_r2 = x and 31
 
 x= get_reg_11_2(&h2411) * 32
 pix_os_av = get_reg_11_2(&h2412) : inc pix_os_av, x
 
 kta_avg = get_reg_11_2(&h2415)
 x=get_reg_11(&h2416)
 kta_scale_1 = x>>5 : kta_scale_2 = x and 31
 
 kv_avg = get_reg_11_2(&h2417)
 x=get_reg_11(&h2418)
 kv_scale_1 = x>>5 : kv_scale_2 = x and 31
 
 x=get_reg_11(&h2419)
 scale_row_1 = (x>>5) + 20 : scale_row_2 = (x and 31) + 20
 x=get_reg_11(&h241A)
 scale_row_3 = (x>>5) + 20 : scale_row_4 = (x and 31) + 20
 x=get_reg_11(&h241B)
 scale_row_5 = (x>>5) + 20 : scale_row_6 = (x and 31) + 20
 
 row1_max=get_reg_11(&h241C)
 row2_max=get_reg_11(&h241D)
 row3_max=get_reg_11(&h241E)
 row4_max=get_reg_11(&h241F)
 row5_max=get_reg_11(&h2420)
 row6_max=get_reg_11(&h2421)
 
 a_ref_row_1 = row1_max / (2^scale_row_1)
 a_ref_row_2 = row2_max / (2^scale_row_2)
 a_ref_row_3 = row3_max / (2^scale_row_3)
 a_ref_row_4 = row4_max / (2^scale_row_4)
 a_ref_row_5 = row5_max / (2^scale_row_5)
 a_ref_row_6 = row6_max / (2^scale_row_6)
 
 ksta = get_reg_11_2(&h2422) / 32768
 
 'emissivity
 em = get_reg_11_2(&h2423) / 512
 
 'general gain
 gain = get_reg_11(&h2424) * 32
 x = get_reg_11(&h2425) : inc gain,x
 Kgain = gain / get_reg_16_2(&h058A)
 
 'ptat
 ptat = get_reg_11(&h2428) * 32
 x = get_reg_11(&h2429) : inc ptat,x
 
 kt_ptat = get_reg_11(&h242A) / 8
 kv_ptat = get_reg_11(&h242B) / 4096
 alpha_ptat = get_reg_11(&h242C) / 128
 
 'control pixel
 alpha_cp_sc = get_reg_11(&h242E)
 alpha_cp = get_reg_11(&h242D) / (2 ^ alpha_cp_sc)
 
 x = get_reg_11(&h242F) * 32
 offset_cp = get_reg_11(&h2430) : inc offset_cp,x : map_16_2(offset_cp)
 gain_cp = get_reg_16_2(&h0588) * Kgain
 
 x = get_reg_11(&h2431)
 kta_cp_sc = x>>6
 kta_cp = x and 63 : if kta_cp > 31 then inc kta_cp,-64
 kta_cp = kta_cp / (2^kta_cp_sc)
 x = get_reg_11(&h2432)
 kv_cp_sc = x>>6
 kv_cp = x and 63 : if kta_cp > 31 then inc kta_cp,-64
 kv_cp = kv_cp / (2^kv_cp_sc)
 
 cp_pix_os = gain_cp - offset_cp*(1+kta_cp*(Ta-25))*(1+Kv_cp*(get_vdd()-3.3))
 
 'IR gradient compensation
 TGC = (get_reg(&h2433) and &h1ff) / 64
 RC_cal = (get_reg(&h2433) and &h600)>>9
 
 'Object temperature scaling
 KsTo_sc = get_reg_11(&h2434)
 KsTo_1 = get_reg_11_2(&h2435) / (2^KsTo_sc)
 KsTo_2 = get_reg_11_2(&h2436) / (2^KsTo_sc)
 KsTo_3 = get_reg_11_2(&h2437) / (2^KsTo_sc)
 KsTo_4 = get_reg_11_2(&h2438) / (2^KsTo_sc)
 KsTo_5 = get_reg_11_2(&h2439) / (2^KsTo_sc)
 KsTo_6 = get_reg_11_2(&h243B) / (2^KsTo_sc)
 KsTo_7 = get_reg_11_2(&h243D) / (2^KsTo_sc)
 KsTo_8 = get_reg_11_2(&h243F) / (2^KsTo_sc)
 CT6 = get_reg_11(&h243A)
 CT7 = get_reg_11(&h243C)
 CT8 = get_reg_11(&h243E)
 
end sub
 
 
 
 ' ------------------- conversion functions -----------------------------
 'this function makes use of the fact that changes in variable x reflect in
 'source variable
 
sub map_16_2(x)   '16 bit 2'th complement
 if x>32767 then inc x,-65536
end sub
 
 
 
 ' ------------------- read chip functions -----------------------------
 'these functions read data from the MLX90641 and do some formatting
 
 
 'return 16 bit value read from MLX90641 registers n, MSB first
function get_reg(n) as integer
 get_reg = read_reg(n)
end function
 
 
 'return an 11 bit value (remove hamming code) from MLX90641 register n
function get_reg_11(n) as integer
 get_reg_11 = read_reg(n) and &h7ff
end function
 
 
 'return a 2'th complement 11 bit value (remove hamming code) from MLX90641 register n
function get_reg_11_2(n) as integer
 get_reg_11_2 = read_reg(n) and &h7ff
 if get_reg_11_2>1023 then inc get_reg_11_2,-2048
end function
 
 
 'return a 16 bit 2'th complement value from MLX90641 register n
function get_reg_16_2(n) as integer
 get_reg_16_2 = read_reg(n)
 if get_reg_16_2>32767 then inc get_reg_16_2,-65536
end function
 
 
function read_reg(n) as integer
 local a%(1))
 if I2Cport=2 then
   i2c2 write SA, 1, 2, n\256, n and 255
   i2c2 read SA,0,2,a%()
 else
   i2c write SA, 1, 2, n\256, n and 255
   i2c read SA,0,2,a%()
 end if
 read_reg=a%(1)+256*a%(0)  
end function
 
 ' ------------------- write chip functions -----------------------------
 'this functions writes a 16 bit value v to MLX90641 register n
 
 
 'write 16 bit value to MLX90641 registers n, MSB first
sub set_reg n,v
 if I2Cport=2 then
   i2c2 write SA,0,4,n\256,n and 255,v\256,v and 255
 else
   i2c write SA,0,4,n\256,n and 255,v\256,v and 255
 end if
end sub
 
 
 '-----------------------matrices for math -----------------------------
 'this sections fills the multiplication matrics that do the up-scaling
 
sub fill_bd_4
 local i
 'matrix b(61,15) for scaling up 2x horizontal, column 61 is not programmed
 for i=0 to 15 : b(4*i,i)=1 : next
 for i=0 to 14 : b(4*i+2,i)=0.5 : b(4*i+2,i+1)=0.5 : next
 for i=0 to 14 : b(4*i+1,i)=0.75 : b(4*i+1,i+1)=0.25 : next
 for i=0 to 14 : b(4*i+3,i)=0.25 : b(4*i+3,i+1)=0.75 : next
 
 if debug then math m_print b():print
 
 'matrix d(11,44) for scaling up 2x vertical
 for i=0 to 11 : d(i,4*i)=1 : next
 for i=0 to 10 : d(i,4*i+2)=0.5 : d(i+1,4*i+2)=0.5 : next
 for i=0 to 10 : d(i,4*i+1)=0.75 : d(i+1,4*i+1)=0.25 : next
 for i=0 to 10 : d(i,4*i+3)=0.25 : d(i+1,4*i+3)=0.75 : next
 
 if debug then math m_print d()

end sub