Updated /raspberry_video/LeptonThread to work for Lepton 3.

software/raspberrypi_video/LeptonThread.cpp

@@ -1,5 +1,5 @@
 #include "LeptonThread.h"
-
+#include "stdio.h"
 #include "Palettes.h"
 #include "SPI.h"
 #include "Lepton_I2C.h"
@@ -8,102 +8,120 @@
 #define PACKET_SIZE_UINT16 (PACKET_SIZE/2)
 #define PACKETS_PER_FRAME 60
 #define FRAME_SIZE_UINT16 (PACKET_SIZE_UINT16*PACKETS_PER_FRAME)
-#define FPS 27;
+#define FPS 27
 
-LeptonThread::LeptonThread() : QThread()
-{
-}
+#define HAND_TEMP_THRESHOLD 8050
 
-LeptonThread::~LeptonThread() {
-}
+int readyToToggle = 1;
+
+LeptonThread::LeptonThread() : QThread(){}
+
+LeptonThread::~LeptonThread() {}
 
 void LeptonThread::run()
 {
-	//create the initial image
-	myImage = QImage(80, 60, QImage::Format_RGB888);
-
-	//open spi port
+	//Create the initial image and open the Spi port.
+	myImage = QImage(160, 120, QImage::Format_RGB888);
 	SpiOpenPort(0);
-
+	uint32_t totalCounts = 0;
+	uint16_t minValue = 65535;
+	uint16_t maxValue = 0;
+	int checkSegment = 1;
+	//Camera Loop
 	while(true) {
-
-		//read data packets from lepton over SPI
 		int resets = 0;
+
+		//Read through one segment, line by line.	
 		for(int j=0;j<PACKETS_PER_FRAME;j++) {
-			//if it's a drop packet, reset j to 0, set to -1 so he'll be at 0 again loop
 			read(spi_cs0_fd, result+sizeof(uint8_t)*PACKET_SIZE*j, sizeof(uint8_t)*PACKET_SIZE);
 			int packetNumber = result[j*PACKET_SIZE+1];
+
+			//Check to make sure packet number is correct, or else the Lepton is out of sync.
+			//We have 750 'resets' before re-syncing.
 			if(packetNumber != j) {
 				j = -1;
 				resets += 1;
 				usleep(1000);
-				//Note: we've selected 750 resets as an arbitrary limit, since there should never be 750 "null" packets between two valid transmissions at the current poll rate
-				//By polling faster, developers may easily exceed this count, and the down period between frames may then be flagged as a loss of sync
 				if(resets == 750) {
 					SpiClosePort(0);
 					usleep(750000);
 					SpiOpenPort(0);
 				}
 			}
 		}
-		if(resets >= 30) {
-			qDebug() << "done reading, resets: " << resets;
-		}
 
-		frameBuffer = (uint16_t *)result;
 		int row, column;
 		uint16_t value;
-		uint16_t minValue = 65535;
-		uint16_t maxValue = 0;
 
-
+		//Obtain the segment number from the header of line 20. It should never be 0.
+		uint8_t segNum = result[20*PACKET_SIZE]>>4;
+		if(segNum != checkSegment){
+			checkSegment = 1;
+			continue;
+		}
+		checkSegment++;
+		if(checkSegment > 4) {
+			checkSegment = 1;
+		}
+
+		 //Iterate through the current segment one line at a time. In this loop, we find the
+		 //minimum and maximum values of the image for AGC.
 		for(int i=0;i<FRAME_SIZE_UINT16;i++) {
-			//skip the first 2 uint16_t's of every packet, they're 4 header bytes
-			if(i % PACKET_SIZE_UINT16 < 2) {
-				continue;
-			}
-
-			//flip the MSB and LSB at the last second
-			int temp = result[i*2];
-			result[i*2] = result[i*2+1];
-			result[i*2+1] = temp;
+			//Skip the header of each line
+			if(i % PACKET_SIZE_UINT16 < 2) continue; 
 
-			value = frameBuffer[i];
+			//Flip the MSB and LSB for correct coloring.
+			frameBuffer[i + (segNum-1)*(FRAME_SIZE_UINT16)] = result[i*2]<<8 | result[i*2+1];
+
+			value = frameBuffer[i+ (segNum-1)*FRAME_SIZE_UINT16];
+			totalCounts += value;
 			if(value > maxValue) {
 				maxValue = value;
 			}
-			if(value < minValue) {
+			if(value < minValue && value > 0) {
 				minValue = value;
 			}
 			column = i % PACKET_SIZE_UINT16 - 2;
-			row = i / PACKET_SIZE_UINT16 ;
+			row = i / (PACKET_SIZE_UINT16 * 2);
 		}
-
+
+		//We only finalize the image for emission once all four segments are in.
+		if(segNum != 4)continue; 	
+
+		//If the difference between Max and Min is 0, we need to get a new frame before emitting.
 		float diff = maxValue - minValue;
-		float scale = 255/diff;
-		QRgb color;
-		for(int i=0;i<FRAME_SIZE_UINT16;i++) {
-			if(i % PACKET_SIZE_UINT16 < 2) {
-				continue;
-			}
-			value = (frameBuffer[i] - minValue) * scale;
-			const int *colormap = colormap_ironblack;
-			color = qRgb(colormap[3*value], colormap[3*value+1], colormap[3*value+2]);
-			column = (i % PACKET_SIZE_UINT16 ) - 2;
-			row = i / PACKET_SIZE_UINT16;
-			myImage.setPixel(column, row, color);
-		}
+		if(diff != 0){
+			float scale = 255/diff;
+			QRgb color;
 
-		//lets emit the signal for update
-		emit updateImage(myImage);
+			//Iterates through the entire frame, all four segments, for colorization.
+			for(int i=0;i<FRAME_SIZE_UINT16*4;i++) {
+				//Skip the header of each line.
+				if(i % PACKET_SIZE_UINT16 < 2) continue; 
+
+				value = (frameBuffer[i] - minValue) * scale;
+				const int *colormap = colormap_ironblack;
+				if(value > 255) value = 255;	
+				color = qRgb(colormap[3*value], colormap[3*value+1], colormap[3*value+2]);
+
+				column = (i % PACKET_SIZE_UINT16 ) - 2;
+				row = i / (PACKET_SIZE_UINT16);
+				int new_row = (row/2);
+				int new_column = (row % 2 == 0) ? column : column + 80 ;
+				myImage.setPixel(new_column, new_row, color);
+			}
 
+			//Emit the finalized image for update.
+			emit updateImage(myImage);
+		}
+		minValue = 65535;
+		maxValue = 0;
+		totalCounts = 0;
 	}
 
-	//finally, close SPI port just bcuz
 	SpiClosePort(0);
 }
 
 void LeptonThread::performFFC() {
-	//perform FFC
 	lepton_perform_ffc();
 }

software/raspberrypi_video/LeptonThread.h

@@ -36,7 +36,8 @@ public slots:
   QImage myImage;
 
   uint8_t result[PACKET_SIZE*PACKETS_PER_FRAME];
-  uint16_t *frameBuffer;
+//  uint16_t* frameBuffer;
+  uint16_t frameBuffer[FRAME_SIZE_UINT16*4];
 
 };