'FLIR Lepton Thermal Camera Driver #1 for P2
'This is a conversion of the P1 version "FLIR_Driver2d.spin"
'Copyright (C) 2015-2019 Raymond Allen.
'MIT License
'This code will read 80x60 images from Lepton camera and then send them via USB serial to PC for processing
'Raw data is 14 bits unless we turn on AGC and then it's 8 bits.

'ver.c  seeing if i can use over long cables by slowing clock
'Note:  Nop's added to last routine to from rate from 27Hz to 9 Hz for long cables
'Ver2a.  Adding another cog driver to convert raw data into 8bpp bitmapped array for faster LCD updates
'Ver2b.  Disabled rate pin as interfered with I2C Lepton control.
'        cogstop now does both cogs



CON  'Pin Settings  'Be sure to change to the pins you are using before use!
  'Note: pin settings backwards here due to vacuum feedthrough mirroring pins...
  PinCs  =4'9'9'11'27
  PinMosi=5'6'8'10'26
  PinMiso=6'7'7'9'25
  PinClk =7'8'6'8'24
  PinSDA =8'4'5'29   'Backwards from hardware version 1
  PinSCL =9'5'4'28

  PinRate=33'7'16  'For testing Will toggle on every good new frame
      'Set this to unused pin or a number >31 if not being used  (do not set to -1!)          



 
CON  'Enumerated assembly driver commands
  #1,CaptureFrame,DummyFrame,SyncStream
        
VAR  'variables this driver
    long     cog,cog2, command
    long     UpdateFlagPointer


PUB Start(pUpdateFlag,pBitMapBuffer,pFrameBuffer):okay     'Start assembly driver and initialize SSD1963

   UpdateFlagPointer:=pUpdateFlag  'Save update flag pointer

   'Start a cog to convert raw data to 8 bpp
   mUpdateFlag:=pUpdateFlag
   mBitmapBuffer:=pBitMapBuffer
   mFrameBuffer:=pFrameBuffer

   
  'Start assembly driver
  ' returns false if no cog available
  Stop
  'cog2 := cognew(@ConverterAssy,0)+1 
  okay := cog := cognew(@StartAssembly, @command) + 1

  
  
PUB Stop
'' Stop PPI Engine - frees a cog
    if cog
       cogstop(cog~ - 1) 
       'cogstop(cog2~ -1)
    command~  


PUB StartCapture(pBuffer)|i,j    'Start reading in frames from camera and store in HUB RAM at location given by pBuffer (does not return)
  i:=pBuffer
  j:=UpdateFlagPointer
  setcommand(CaptureFrame,@i)
  if (i<=j)
  return 

PUB GetDummyFrame       'Read in dummy frame from camera but don't save it
  setcommand(DummyFrame,0)
  return
  
PUB Sync       'Sychronize with video stream
  setcommand(SyncStream,0)
  return
       
PUB delayms(ms)
  waitcnt(cnt+(clkfreq/1000)*ms)  

  
PUB setcommand(cmd, argptr)
    command := cmd << 16 + argptr                       'write command and pointer
    repeat while command                                'wait for command to be cleared, signifying receipt



DAT 'Assembly to convert raw data to 8bpp

org 0
ConverterAssy
              'jmp       #ConverterAssy
                
WaitZero
       'wait for new frame
       
              rdlong    x1,mUpdateFlag
              cmp       x1,#0 wz,wc
        if_ne jmp       #WaitZero  'first wait for zero
WaitOne        
              rdlong    x1,mUpdateFlag
              cmp       x1,#1 wz,wc
        if_ne jmp       #WaitOne  'then wait for one
               
              'Convert using previous max and min while checking for new max and min
              mov       mNewMax,#0
              mov       mNewMin,xallones
              mov       x1,mFrameBuffer
              mov       x2,mBitmapBuffer
              mov       x3,#60
XLineLoop
              add       x1,#4 'skip first two words
              mov       x4,#80
XColumnLoop
              rdword    x5,x1  'read in a value
              add       x1,#2
              'see if it's the new max or min
              cmp       x5,mNewMax wz,wc
        if_a  mov       mNewMax,x5
              cmp       x5,mNewMin wz,wc
        if_b  mov       mNewMin,x5
              'convert to 8bpp
              mov       y2,x5
              sub       y2,mOldMin
              cmps      y2,#0   wz,wc
        if_b  mov       y2,#0
              mov       y1,#255
              call      #Multiply
              mov       y2,y1
              mov       y1,mDiff
              call      #Divide
              and       y2,wordMask
              'make sure result in range
              cmp       y2,#255 wz,wc
        if_a  mov       y2,#255

              wrbyte    y2,x2
              add       x2,#1
              djnz      x4,#XColumnLoop
              djnz      x3,#XLineLoop

              'adjust min and max
              mov       mOldMax,mNewMax
              mov       mOldMin,mNewMin
              mov       mDiff,mOldMax
              sub       mDiff,mOldMin
Xtest
              'jmp       #XTest
              jmp       #WaitZero 'start over
              
                 
              

DAT' Multiply y1 * y2 (y1, y2 = 16bits) result in y1 (adapted from vga.spin)
'
multiply      shl     y2,#16-1

              mov     y3,#16
.loop         shr     y1,#1           wc
        if_c  add     y1,y2
              djnz    y3,#.loop

multiply_ret  ret  

' Divide y2[31..0] by y1[15..0] (y1[16] must be 0)
' on exit, quotient is in y2[15..0] and remainder is in y2[31..16]
'
divide        shl       y1,#15 'get divisor into y[30..15]
              mov       y3,#16 'ready for 16 quotient bits
.loop         cmpsub    y2,y1 wc 'y =< x? Subtract it, quotient bit in c
              rcl       y2,#1 'rotate c into quotient, shift dividend
              djnz      y3,#.loop 'loop until done
divide_ret    ret 'quotient in y2[15..0],
'remainder in y2[31..16]

DAT
'################################################################################################################  
mDiff                   long    2000
mOldMax                 long    9000
mOldMin                 long    7000
wordMask                long    $FFFF
mNewMax                 long    0
mNewMin                 long    0
mUpdateFlag             long    0 'pointer to update flag
mBitmapBuffer           long    0 'pointer to output 8bpp array
mFrameBuffer            long    0 'pointer to input 14bpp array            

xallones                long    -1 
{
########################### Undefined data ###########################
}

'temp variables
y1                      res     1
y2                      res     1
y3                      res     1
x1                      res     1                       '    
x2                      res     1                       '    
x3                      res     1                       '   
x4                      res     1                       '   
x5                      res     1                       '   
x6                      res     1
x7                      res     1
x8                      res     1
x9                      res     1
x10                     res     1

'################################################################################################################   

DAT   'Assembly Driver

org  0
StartAssembly
              jmp       #init   'Jump over mask definitions

'Inserting mask definitions here for clarity (instruction causes jump over this section)
CS_mask       long 1<<PinCS              ' Chip Select Signal
MOSI_mask     long 1<<PinMOSI            ' Slave In
MISO_mask     long 1<<PinMISO            ' Slave Out
CLK_mask      long 1<<PinCLK             ' Clock

Rate_mask     long 1<<PinRate            'testing video rate

Pin_mask      long  1<<PinCS+1<<PinMOSI+1<<PinCLK  +1<<PinRate  'all but MISO to be outputs

 
' Assembly Control Engine - main loop
'
init          mov       outa, Pin_mask       'Start with all control pins set high
              andn      outa, MOSI_mask         'Lepton wants us to keep MOSI low. (it's not used).
              mov       dira, Pin_mask
              
              
loop          rdlong  t1,ptra          wz                'wait for command
        if_z  jmp     #loop
              setd    .arg,#arg0                        'get 8 arguments ; arg0 to arg7
              mov     t2,t1                             '    ‘
              mov     t3,#8                             'º���œ 
.arg          rdlong  arg0,t2
              add     .arg,d0
              add     t2,#4
              djnz    t3,#.arg
              mov     address,t1                        'preserve address location for passing
                                                        'variables back to Spin language.
                                                        
              wrlong  zero,ptra                          'zero command to signify command received
              ror     t1,#16+2                          'lookup command address
              add     t1,#jumps
              sets    .table,t1
              rol     t1,#2
              shl     t1,#3
.table        mov     t2,0
              shr     t2,t1
              and     t2,#$FF
              jmp     t2                                'jump to command
jumps         byte    0                                 '0
              byte    CaptureFrame_                     '1
              

DummyOp_              
NotUsed_      jmp     #loop







DAT 'CaptureFrame  ################################################################################################################
CaptureFrame_    'This repeatedly captures frames and sends to hub address provided by first argument
              mov       t1,arg0  'address of HUB buffer that we will write to

              'pause before every frame
              'mov       t7,cnt
              'add       t7,delaycnt
              'waitcnt   t7,#0
              waitx      delaycnt


              
              mov       t2,#60 'Doing 60 lines of video
FrameLoop
              andn      outa,CS_mask  'bring chip select low to start transfer

              'read in ID and CRC
              call      #ShiftWordInSub
              mov       t7,t5 'save ID
              wrword    t5,t1 'write word to HUB
              add       t1,#2 'prepare for next word write
              
              mov       t3,#81 'reading 82 words per line
LineLoop      'There are 60 packets per frame (one for each line), each is 1312 bits long (82 words [1 is ID, 1 is CRS, next 80 are pixels] )
                                   
              call      #ShiftWordInSub
              wrword    t5,t1 'write word to HUB
              add       t1,#2 'prepare for next word write
              djnz      t3,#LineLoop            
              
              or        outa,CS_mask 'bring CS high for end of line


              'See if packet is OK, or need to sleep and try again
              mov       t8,t7 'save it
              and       t7,DiscardFlags
              cmp       t7,DiscardFlags wz,wc
        if_e  jmp       #CaptureFrame_
              'wait for packet#0 on first round
              cmp       t2,#60 wz,wc
        if_ne jmp       #EndFrameLoop
              and       t8,#$FF
              cmp       t8,#0 wz,wc
        if_ne jmp       #CaptureFrame_  'start over if not first packet         

              wrlong    zero, arg1  'Reset new frame flag on start of new frame
EndFrameLoop        
              djnz      t2,#FrameLoop 'Start over until all lines read in


              'for seeing frame rate on oscilloscope:
              or        outa,Rate_mask    'Set rate pin to see how fast we are capturing frames

              wrlong    one, arg1  'Set new frame flag
               
              'wait and then start over
              'mov       t7,cnt
              'add       t7,delaycnt2
              'waitcnt   t7,#0
              waitx     delaycnt2

              andn      outa,Rate_mask     'Clear rate pin
testing
           
              jmp       #CaptureFrame_   'Start Over



DAT ShiftWordInSub  'read word into t5 using t4 as counter
              mov       t4,#16
WordLoop      'read in a word and then write to HUB
              andn      outa,clk_mask 'bring clock low
              nop
              nop
              nop 
              test      MISO_mask,ina wc  'get data into carry
              rcl       t5,#1 'shift into t5
              or        outa,clk_mask 'bring clock high
              nop
              nop
              djnz      t4,#WordLoop 'do this 16 times
              'The above read loop has 5 instructions:  20 MIPS/5 --> 4 MHz read
ShiftWordInSub_RET
              RET              




DAT 'working variables
{
########################### Defined data ###########################
}
 

zero                    long    0                       'constants
one                     long    1
d0                      long    $200
allones                 long    -1
DiscardFlags            long    $F00
delaycnt                long    20_000*8  '*8 delays for P2
delaycnt2               long    1_000_000*8 

{
########################### Undefined data ###########################
}
                                                        'temp variables
notready                res     1
t1                      res     1                       '    
t2                      res     1                       '    
t3                      res     1                       '   
t4                      res     1                       '   
t5                      res     1                       '   
t6                      res     1
t7                      res     1
t8                      res     1
t9                      res     1
t10                     res     1
address                 res     1                       '     Used to hold return address of first Argument passed

arg0                    res     1                       'arguments passed to/from high-level Spin
arg1                    res     1
arg2                    res     1
arg3                    res     1
arg4                    res     1
arg5                    res     1
arg6                    res     1
arg7                    res     1





              

fit 496